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