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