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