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
message(const char * fmt,...)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
register_sdp(struct bt_audio_receiver * r)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
unregister_sdp(struct bt_audio_receiver * r)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
start_listen(struct bt_audio_receiver * r)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
stop_listen(struct bt_audio_receiver * r)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
close_connection(struct bt_audio_connection * c)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 *
wait_for_connection(struct bt_audio_receiver * r)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
setup_oss(struct bt_audio_connection * c)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
process_connection(struct bt_audio_connection * c)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
usage(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
main(int argc,char ** argv)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