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