1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause 3 * 4 * Copyright (c) 2019 Google LLC, written by Richard Kralovic <riso@google.com> 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25 * SUCH DAMAGE. 26 */ 27 28 #include <sys/types.h> 29 #include <sys/rtprio.h> 30 #include <sys/soundcard.h> 31 32 #include <dlfcn.h> 33 #include <err.h> 34 #include <fcntl.h> 35 #include <stdarg.h> 36 #include <stdint.h> 37 #include <stdio.h> 38 #include <stdlib.h> 39 #include <string.h> 40 #include <sysexits.h> 41 #include <time.h> 42 #include <unistd.h> 43 #include <poll.h> 44 #include <getopt.h> 45 46 #define L2CAP_SOCKET_CHECKED 47 #include <bluetooth.h> 48 #include <sdp.h> 49 50 #include "avdtp_signal.h" 51 #include "bt.h" 52 53 static int (*bt_receive_f)(struct bt_config *, void *, int, int); 54 static int (*avdtpACPHandlePacket_f)(struct bt_config *cfg); 55 static void (*avdtpACPFree_f)(struct bt_config *); 56 57 static int bt_in_background; 58 59 static void 60 message(const char *fmt,...) 61 { 62 va_list list; 63 64 if (bt_in_background) 65 return; 66 67 va_start(list, fmt); 68 vfprintf(stderr, fmt, list); 69 va_end(list); 70 } 71 72 struct bt_audio_receiver { 73 const char *devname; 74 const char *sdp_socket_path; 75 uint16_t l2cap_psm; 76 int fd_listen; 77 void *sdp_session; 78 uint32_t sdp_handle; 79 }; 80 81 static int 82 register_sdp(struct bt_audio_receiver *r) 83 { 84 struct sdp_audio_sink_profile record = {}; 85 86 r->sdp_session = sdp_open_local(r->sdp_socket_path); 87 if (r->sdp_session == NULL || sdp_error(r->sdp_session)) { 88 sdp_close(r->sdp_session); 89 r->sdp_session = NULL; 90 return (0); 91 } 92 93 record.psm = r->l2cap_psm; 94 record.protover = 0x100; 95 record.features = 0x01; /* player only */ 96 97 if (sdp_register_service(r->sdp_session, SDP_SERVICE_CLASS_AUDIO_SINK, 98 NG_HCI_BDADDR_ANY, (const uint8_t *)&record, sizeof(record), 99 &r->sdp_handle)) { 100 message("SDP failed to register: %s\n", 101 strerror(sdp_error(r->sdp_session))); 102 sdp_close(r->sdp_session); 103 r->sdp_session = NULL; 104 return (0); 105 } 106 return (1); 107 } 108 109 static void 110 unregister_sdp(struct bt_audio_receiver *r) 111 { 112 sdp_unregister_service(r->sdp_session, r->sdp_handle); 113 sdp_close(r->sdp_session); 114 r->sdp_session = NULL; 115 } 116 117 static int 118 start_listen(struct bt_audio_receiver *r) 119 { 120 struct sockaddr_l2cap addr = {}; 121 122 r->fd_listen = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP); 123 if (r->fd_listen < 0) 124 return (0); 125 126 addr.l2cap_len = sizeof(addr); 127 addr.l2cap_family = AF_BLUETOOTH; 128 addr.l2cap_psm = r->l2cap_psm; 129 130 if (bind(r->fd_listen, (struct sockaddr *)&addr, sizeof(addr)) < 0 || 131 listen(r->fd_listen, 4) < 0) { 132 close(r->fd_listen); 133 return (0); 134 } 135 return (1); 136 } 137 138 static void 139 stop_listen(struct bt_audio_receiver *r) 140 { 141 close(r->fd_listen); 142 } 143 144 struct bt_audio_connection { 145 struct bt_audio_receiver *r; 146 struct sockaddr_l2cap peer_addr; 147 struct bt_config cfg; 148 int oss_fd; 149 }; 150 151 static void 152 close_connection(struct bt_audio_connection *c) 153 { 154 avdtpACPFree_f(&c->cfg); 155 if (c->cfg.fd != -1) 156 close(c->cfg.fd); 157 if (c->cfg.hc != -1) 158 close(c->cfg.hc); 159 if (c->oss_fd != -1) 160 close(c->oss_fd); 161 free(c); 162 } 163 164 static struct bt_audio_connection * 165 wait_for_connection(struct bt_audio_receiver *r) 166 { 167 struct bt_audio_connection *c = 168 malloc(sizeof(struct bt_audio_connection)); 169 socklen_t addrlen; 170 171 memset(c, 0, sizeof(*c)); 172 173 c->r = r; 174 c->cfg.fd = -1; 175 c->oss_fd = -1; 176 177 addrlen = sizeof(c->peer_addr); 178 c->cfg.hc = accept(r->fd_listen, (struct sockaddr *)&c->peer_addr, &addrlen); 179 180 message("Accepted control connection, %d\n", c->cfg.hc); 181 if (c->cfg.hc < 0) { 182 close_connection(c); 183 return NULL; 184 } 185 c->cfg.sep = 0; /* to be set later */ 186 c->cfg.media_Type = mediaTypeAudio; 187 c->cfg.chmode = MODE_DUAL; 188 c->cfg.aacMode1 = 0; /* TODO: support AAC */ 189 c->cfg.aacMode2 = 0; 190 c->cfg.acceptor_state = acpInitial; 191 192 return (c); 193 } 194 195 static void 196 setup_oss(struct bt_audio_connection *c) 197 { 198 c->oss_fd = open(c->r->devname, O_WRONLY); 199 200 if (c->oss_fd < 0) 201 goto err; 202 203 int v; 204 205 switch (c->cfg.chmode) { 206 case MODE_STEREO: 207 case MODE_JOINT: 208 case MODE_DUAL: 209 v = 2; 210 break; 211 case MODE_MONO: 212 v = 1; 213 break; 214 default: 215 message("Wrong chmode\n"); 216 goto err; 217 } 218 219 if (ioctl(c->oss_fd, SNDCTL_DSP_CHANNELS, &v) < 0) { 220 message("SNDCTL_DSP_CHANNELS failed\n"); 221 goto err; 222 } 223 v = AFMT_S16_NE; 224 if (ioctl(c->oss_fd, SNDCTL_DSP_SETFMT, &v) < 0) { 225 message("SNDCTL_DSP_SETFMT failed\n"); 226 goto err; 227 } 228 switch (c->cfg.freq) { 229 case FREQ_16K: 230 v = 16000; 231 break; 232 case FREQ_32K: 233 v = 32000; 234 break; 235 case FREQ_44_1K: 236 v = 44100; 237 break; 238 case FREQ_48K: 239 v = 48000; 240 break; 241 default: 242 message("Wrong freq\n"); 243 goto err; 244 } 245 246 if (ioctl(c->oss_fd, SNDCTL_DSP_SPEED, &v) < 0) { 247 message("SNDCTL_DSP_SETFMT failed\n"); 248 goto err; 249 } 250 v = (2 << 16) | 15; /* 2 fragments of 32k each */ 251 if (ioctl(c->oss_fd, SNDCTL_DSP_SETFRAGMENT, &v) < 0) { 252 message("SNDCTL_DSP_SETFRAGMENT failed\n"); 253 goto err; 254 } 255 return; 256 257 err: 258 c->oss_fd = -1; 259 message("Cannot open oss device %s\n", c->r->devname); 260 } 261 262 static void 263 process_connection(struct bt_audio_connection *c) 264 { 265 struct pollfd pfd[3] = {}; 266 time_t oss_attempt = 0; 267 268 while (c->cfg.acceptor_state != acpStreamClosed) { 269 int np; 270 271 pfd[0].fd = c->r->fd_listen; 272 pfd[0].events = POLLIN | POLLRDNORM; 273 pfd[0].revents = 0; 274 275 pfd[1].fd = c->cfg.hc; 276 pfd[1].events = POLLIN | POLLRDNORM; 277 pfd[1].revents = 0; 278 279 pfd[2].fd = c->cfg.fd; 280 pfd[2].events = POLLIN | POLLRDNORM; 281 pfd[2].revents = 0; 282 283 if (c->cfg.fd != -1) 284 np = 3; 285 else 286 np = 2; 287 288 if (poll(pfd, np, INFTIM) < 0) 289 return; 290 291 if (pfd[1].revents != 0) { 292 int retval; 293 294 message("Handling packet: state = %d, ", 295 c->cfg.acceptor_state); 296 retval = avdtpACPHandlePacket_f(&c->cfg); 297 message("retval = %d\n", retval); 298 if (retval < 0) 299 return; 300 } 301 if (pfd[0].revents != 0) { 302 socklen_t addrlen = sizeof(c->peer_addr); 303 int fd = accept4(c->r->fd_listen, 304 (struct sockaddr *)&c->peer_addr, &addrlen, 305 SOCK_NONBLOCK); 306 307 if (fd < 0) 308 return; 309 310 if (c->cfg.fd < 0) { 311 if (c->cfg.acceptor_state == acpStreamOpened) { 312 socklen_t mtusize = sizeof(uint16_t); 313 c->cfg.fd = fd; 314 315 if (getsockopt(c->cfg.fd, SOL_L2CAP, SO_L2CAP_IMTU, &c->cfg.mtu, &mtusize) == -1) { 316 message("Could not get MTU size\n"); 317 return; 318 } 319 320 int temp = c->cfg.mtu * 32; 321 322 if (setsockopt(c->cfg.fd, SOL_SOCKET, SO_RCVBUF, &temp, sizeof(temp)) == -1) { 323 message("Could not set send buffer size\n"); 324 return; 325 } 326 327 temp = 1; 328 if (setsockopt(c->cfg.fd, SOL_SOCKET, SO_RCVLOWAT, &temp, sizeof(temp)) == -1) { 329 message("Could not set low water mark\n"); 330 return; 331 } 332 message("Accepted data connection, %d\n", c->cfg.fd); 333 } 334 } else { 335 close(fd); 336 } 337 } 338 if (pfd[2].revents != 0) { 339 uint8_t data[65536]; 340 int len; 341 342 if ((len = bt_receive_f(&c->cfg, data, sizeof(data), 0)) < 0) { 343 return; 344 } 345 if (c->cfg.acceptor_state != acpStreamSuspended && 346 c->oss_fd < 0 && 347 time(NULL) != oss_attempt) { 348 message("Trying to open dsp\n"); 349 setup_oss(c); 350 oss_attempt = time(NULL); 351 } 352 if (c->oss_fd > -1) { 353 uint8_t *end = data + len; 354 uint8_t *ptr = data; 355 unsigned delay; 356 unsigned jitter_limit; 357 358 switch (c->cfg.freq) { 359 case FREQ_16K: 360 jitter_limit = (16000 / 20); 361 break; 362 case FREQ_32K: 363 jitter_limit = (32000 / 20); 364 break; 365 case FREQ_44_1K: 366 jitter_limit = (44100 / 20); 367 break; 368 default: 369 jitter_limit = (48000 / 20); 370 break; 371 } 372 373 if (c->cfg.chmode == MODE_MONO) { 374 if (len >= 2 && 375 ioctl(c->oss_fd, SNDCTL_DSP_GETODELAY, &delay) == 0 && 376 delay < (jitter_limit * 2)) { 377 uint8_t jitter[jitter_limit * 4] __aligned(4); 378 size_t x; 379 380 /* repeat last sample */ 381 for (x = 0; x != sizeof(jitter); x++) 382 jitter[x] = ptr[x % 2]; 383 384 write(c->oss_fd, jitter, sizeof(jitter)); 385 } 386 } else { 387 if (len >= 4 && 388 ioctl(c->oss_fd, SNDCTL_DSP_GETODELAY, &delay) == 0 && 389 delay < (jitter_limit * 4)) { 390 uint8_t jitter[jitter_limit * 8] __aligned(4); 391 size_t x; 392 393 /* repeat last sample */ 394 for (x = 0; x != sizeof(jitter); x++) 395 jitter[x] = ptr[x % 4]; 396 397 write(c->oss_fd, jitter, sizeof(jitter)); 398 } 399 } 400 while (ptr != end) { 401 int written = write(c->oss_fd, ptr, end - ptr); 402 403 if (written < 0) { 404 if (errno != EINTR && errno != EAGAIN) 405 break; 406 written = 0; 407 } 408 ptr += written; 409 } 410 if (ptr != end) { 411 message("Not all written, closing dsp\n"); 412 close(c->oss_fd); 413 c->oss_fd = -1; 414 oss_attempt = time(NULL); 415 } 416 } 417 } 418 419 if (c->cfg.acceptor_state == acpStreamSuspended && 420 c->oss_fd > -1) { 421 close(c->oss_fd); 422 c->oss_fd = -1; 423 } 424 } 425 } 426 427 static struct option bt_speaker_opts[] = { 428 {"device", required_argument, NULL, 'd'}, 429 {"sdp_socket_path", required_argument, NULL, 'p'}, 430 {"rtprio", required_argument, NULL, 'i'}, 431 {"background", no_argument, NULL, 'B'}, 432 {"help", no_argument, NULL, 'h'}, 433 {NULL, 0, NULL, 0} 434 }; 435 436 static void 437 usage(void) 438 { 439 fprintf(stderr, "Usage: virtual_bt_speaker -d /dev/dsp\n" 440 "\t" "-d, --device [device]\n" 441 "\t" "-p, --sdp_socket_path [path]\n" 442 "\t" "-i, --rtprio [priority]\n" 443 "\t" "-B, --background\n" 444 ); 445 exit(EX_USAGE); 446 } 447 448 int 449 main(int argc, char **argv) 450 { 451 struct bt_audio_receiver r = {}; 452 struct rtprio rtp = {}; 453 void *hdl; 454 int ch; 455 456 r.devname = NULL; 457 r.sdp_socket_path = NULL; 458 r.l2cap_psm = SDP_UUID_PROTOCOL_AVDTP; 459 460 while ((ch = getopt_long(argc, argv, "p:i:d:Bh", bt_speaker_opts, NULL)) != -1) { 461 switch (ch) { 462 case 'd': 463 r.devname = optarg; 464 break; 465 case 'p': 466 r.sdp_socket_path = optarg; 467 break; 468 case 'B': 469 bt_in_background = 1; 470 break; 471 case 'i': 472 rtp.type = RTP_PRIO_REALTIME; 473 rtp.prio = atoi(optarg); 474 if (rtprio(RTP_SET, getpid(), &rtp) != 0) { 475 message("Cannot set realtime priority\n"); 476 } 477 break; 478 default: 479 usage(); 480 break; 481 } 482 } 483 484 if (r.devname == NULL) 485 errx(EX_USAGE, "No devicename specified"); 486 487 if (bt_in_background) { 488 if (daemon(0, 0) != 0) 489 errx(EX_SOFTWARE, "Cannot become daemon"); 490 } 491 492 if ((hdl = dlopen("/usr/lib/virtual_oss/voss_bt.so", RTLD_NOW)) == NULL) 493 errx(1, "%s", dlerror()); 494 if ((bt_receive_f = dlsym(hdl, "bt_receive")) == NULL) 495 goto err_dlsym; 496 if ((avdtpACPHandlePacket_f = dlsym(hdl, "avdtpACPHandlePacket")) == 497 NULL) 498 goto err_dlsym; 499 if ((avdtpACPFree_f = dlsym(hdl, "avdtpACPFree")) == NULL) 500 goto err_dlsym; 501 502 while (1) { 503 message("Starting to listen\n"); 504 if (!start_listen(&r)) { 505 message("Failed to initialize server socket\n"); 506 goto err_listen; 507 } 508 message("Registering service via SDP\n"); 509 if (!register_sdp(&r)) { 510 message("Failed to register in SDP\n"); 511 goto err_sdp; 512 } 513 while (1) { 514 message("Waiting for connection...\n"); 515 struct bt_audio_connection *c = wait_for_connection(&r); 516 517 if (c == NULL) { 518 message("Failed to get connection\n"); 519 goto err_conn; 520 } 521 message("Got connection...\n"); 522 523 process_connection(c); 524 525 message("Connection finished...\n"); 526 527 close_connection(c); 528 } 529 err_conn: 530 message("Unregistering service\n"); 531 unregister_sdp(&r); 532 err_sdp: 533 stop_listen(&r); 534 err_listen: 535 sleep(5); 536 } 537 return (0); 538 539 err_dlsym: 540 warnx("%s", dlerror()); 541 dlclose(hdl); 542 exit(EXIT_FAILURE); 543 } 544