1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2006-2007 Nokia Corporation
6 * Copyright (C) 2004-2008 Marcel Holtmann <marcel@holtmann.org>
7 *
8 *
9 * This library is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 2.1 of the License, or (at your option) any later version.
13 *
14 * This library is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 * Lesser General Public License for more details.
18 *
19 * You should have received a copy of the GNU Lesser General Public
20 * License along with this library; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24
25 #ifdef HAVE_CONFIG_H
26 #include <config.h>
27 #endif
28
29 #include <stdint.h>
30 #include <sys/socket.h>
31 #include <sys/un.h>
32 #include <signal.h>
33 #include <limits.h>
34 #include <fcntl.h>
35 #include <unistd.h>
36 #include <pthread.h>
37
38 #include <netinet/in.h>
39 #include <sys/poll.h>
40 #include <sys/prctl.h>
41
42 #include <bluetooth/bluetooth.h>
43 #include <bluetooth/l2cap.h>
44
45 #include "ipc.h"
46 #include "sbc.h"
47 #include "rtp.h"
48 #include "liba2dp.h"
49
50 #define LOG_NDEBUG 0
51 #define LOG_TAG "A2DP"
52 #include <utils/Log.h>
53
54 #define ENABLE_DEBUG
55 /* #define ENABLE_VERBOSE */
56 /* #define ENABLE_TIMING */
57
58 #define BUFFER_SIZE 2048
59
60 #ifdef ENABLE_DEBUG
61 #define DBG LOGD
62 #else
63 #define DBG(fmt, arg...)
64 #endif
65
66 #ifdef ENABLE_VERBOSE
67 #define VDBG LOGV
68 #else
69 #define VDBG(fmt, arg...)
70 #endif
71
72 #ifndef MIN
73 # define MIN(x, y) ((x) < (y) ? (x) : (y))
74 #endif
75
76 #ifndef MAX
77 # define MAX(x, y) ((x) > (y) ? (x) : (y))
78 #endif
79
80 #define MAX_BITPOOL 64
81 #define MIN_BITPOOL 2
82
83 #define ERR LOGE
84
85 /* Number of packets to buffer in the stream socket */
86 #define PACKET_BUFFER_COUNT 10
87
88 /* timeout in milliseconds to prevent poll() from hanging indefinitely */
89 #define POLL_TIMEOUT 1000
90
91 /* milliseconds of unsucessfull a2dp packets before we stop trying to catch up
92 * on write()'s and fall-back to metered writes */
93 #define CATCH_UP_TIMEOUT 200
94
95 /* timeout in milliseconds for a2dp_write */
96 #define WRITE_TIMEOUT 1000
97
98 /* timeout in seconds for command socket recv() */
99 #define RECV_TIMEOUT 5
100
101
102 typedef enum {
103 A2DP_STATE_NONE = 0,
104 A2DP_STATE_INITIALIZED,
105 A2DP_STATE_CONFIGURING,
106 A2DP_STATE_CONFIGURED,
107 A2DP_STATE_STARTING,
108 A2DP_STATE_STARTED,
109 A2DP_STATE_STOPPING,
110 } a2dp_state_t;
111
112 typedef enum {
113 A2DP_CMD_NONE = 0,
114 A2DP_CMD_INIT,
115 A2DP_CMD_CONFIGURE,
116 A2DP_CMD_START,
117 A2DP_CMD_STOP,
118 A2DP_CMD_QUIT,
119 } a2dp_command_t;
120
121 struct bluetooth_data {
122 unsigned int link_mtu; /* MTU for transport channel */
123 struct pollfd stream; /* Audio stream filedescriptor */
124 struct pollfd server; /* Audio daemon filedescriptor */
125 a2dp_state_t state; /* Current A2DP state */
126 a2dp_command_t command; /* Current command for a2dp_thread */
127 pthread_t thread;
128 pthread_mutex_t mutex;
129 int started;
130 pthread_cond_t thread_start;
131 pthread_cond_t thread_wait;
132 pthread_cond_t client_wait;
133
134 sbc_capabilities_t sbc_capabilities;
135 sbc_t sbc; /* Codec data */
136 int frame_duration; /* length of an SBC frame in microseconds */
137 int codesize; /* SBC codesize */
138 int samples; /* Number of encoded samples */
139 uint8_t buffer[BUFFER_SIZE]; /* Codec transfer buffer */
140 int count; /* Codec transfer buffer counter */
141
142 int nsamples; /* Cumulative number of codec samples */
143 uint16_t seq_num; /* Cumulative packet sequence */
144 int frame_count; /* Current frames in buffer*/
145
146 char address[20];
147 int rate;
148 int channels;
149
150 /* used for pacing our writes to the output socket */
151 uint64_t next_write;
152 };
153
get_microseconds()154 static uint64_t get_microseconds()
155 {
156 struct timespec now;
157 clock_gettime(CLOCK_MONOTONIC, &now);
158 return (now.tv_sec * 1000000UL + now.tv_nsec / 1000UL);
159 }
160
161 #ifdef ENABLE_TIMING
print_time(const char * message,uint64_t then,uint64_t now)162 static void print_time(const char* message, uint64_t then, uint64_t now)
163 {
164 DBG("%s: %lld us", message, now - then);
165 }
166 #endif
167
168 static int audioservice_send(struct bluetooth_data *data, const bt_audio_msg_header_t *msg);
169 static int audioservice_expect(struct bluetooth_data *data, bt_audio_msg_header_t *outmsg,
170 int expected_type);
171 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data);
172 static void set_state(struct bluetooth_data *data, a2dp_state_t state);
173
174
bluetooth_close(struct bluetooth_data * data)175 static void bluetooth_close(struct bluetooth_data *data)
176 {
177 DBG("bluetooth_close");
178 if (data->server.fd >= 0) {
179 bt_audio_service_close(data->server.fd);
180 data->server.fd = -1;
181 }
182
183 if (data->stream.fd >= 0) {
184 close(data->stream.fd);
185 data->stream.fd = -1;
186 }
187
188 data->state = A2DP_STATE_NONE;
189 }
190
l2cap_set_flushable(int fd,int flushable)191 static int l2cap_set_flushable(int fd, int flushable)
192 {
193 int flags;
194 socklen_t len;
195
196 len = sizeof(flags);
197 if (getsockopt(fd, SOL_BLUETOOTH, BT_FLUSHABLE, &flags, &len) < 0)
198 return -errno;
199
200 if (flushable) {
201 if (flags == BT_FLUSHABLE_ON)
202 return 0;
203 flags = BT_FLUSHABLE_ON;
204 } else {
205 if (flags == BT_FLUSHABLE_OFF)
206 return 0;
207 flags = BT_FLUSHABLE_OFF;
208 }
209
210 if (setsockopt(fd, SOL_BLUETOOTH, L2CAP_LM, &flags, sizeof(flags)) < 0)
211 return -errno;
212
213 return 0;
214 }
215
bluetooth_start(struct bluetooth_data * data)216 static int bluetooth_start(struct bluetooth_data *data)
217 {
218 char c = 'w';
219 char buf[BT_SUGGESTED_BUFFER_SIZE];
220 struct bt_start_stream_req *start_req = (void*) buf;
221 struct bt_start_stream_rsp *start_rsp = (void*) buf;
222 struct bt_new_stream_ind *streamfd_ind = (void*) buf;
223 int opt_name, err, bytes;
224
225 DBG("bluetooth_start");
226 data->state = A2DP_STATE_STARTING;
227 /* send start */
228 memset(start_req, 0, BT_SUGGESTED_BUFFER_SIZE);
229 start_req->h.type = BT_REQUEST;
230 start_req->h.name = BT_START_STREAM;
231 start_req->h.length = sizeof(*start_req);
232
233
234 err = audioservice_send(data, &start_req->h);
235 if (err < 0)
236 goto error;
237
238 start_rsp->h.length = sizeof(*start_rsp);
239 err = audioservice_expect(data, &start_rsp->h, BT_START_STREAM);
240 if (err < 0)
241 goto error;
242
243 streamfd_ind->h.length = sizeof(*streamfd_ind);
244 err = audioservice_expect(data, &streamfd_ind->h, BT_NEW_STREAM);
245 if (err < 0)
246 goto error;
247
248 data->stream.fd = bt_audio_service_get_data_fd(data->server.fd);
249 if (data->stream.fd < 0) {
250 ERR("bt_audio_service_get_data_fd failed, errno: %d", errno);
251 err = -errno;
252 goto error;
253 }
254 l2cap_set_flushable(data->stream.fd, 1);
255 data->stream.events = POLLOUT;
256
257 /* set our socket buffer to the size of PACKET_BUFFER_COUNT packets */
258 bytes = data->link_mtu * PACKET_BUFFER_COUNT;
259 setsockopt(data->stream.fd, SOL_SOCKET, SO_SNDBUF, &bytes,
260 sizeof(bytes));
261
262 data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
263 data->frame_count = 0;
264 data->samples = 0;
265 data->nsamples = 0;
266 data->seq_num = 0;
267 data->frame_count = 0;
268 data->next_write = 0;
269
270 set_state(data, A2DP_STATE_STARTED);
271 return 0;
272
273 error:
274 /* close bluetooth connection to force reinit and reconfiguration */
275 if (data->state == A2DP_STATE_STARTING) {
276 bluetooth_close(data);
277 /* notify client that thread is ready for next command */
278 pthread_cond_signal(&data->client_wait);
279 }
280 return err;
281 }
282
bluetooth_stop(struct bluetooth_data * data)283 static int bluetooth_stop(struct bluetooth_data *data)
284 {
285 char buf[BT_SUGGESTED_BUFFER_SIZE];
286 struct bt_stop_stream_req *stop_req = (void*) buf;
287 struct bt_stop_stream_rsp *stop_rsp = (void*) buf;
288 int err;
289
290 DBG("bluetooth_stop");
291
292 data->state = A2DP_STATE_STOPPING;
293 l2cap_set_flushable(data->stream.fd, 0);
294 if (data->stream.fd >= 0) {
295 close(data->stream.fd);
296 data->stream.fd = -1;
297 }
298
299 /* send stop request */
300 memset(stop_req, 0, BT_SUGGESTED_BUFFER_SIZE);
301 stop_req->h.type = BT_REQUEST;
302 stop_req->h.name = BT_STOP_STREAM;
303 stop_req->h.length = sizeof(*stop_req);
304
305 err = audioservice_send(data, &stop_req->h);
306 if (err < 0)
307 goto error;
308
309 stop_rsp->h.length = sizeof(*stop_rsp);
310 err = audioservice_expect(data, &stop_rsp->h, BT_STOP_STREAM);
311 if (err < 0)
312 goto error;
313
314 error:
315 if (data->state == A2DP_STATE_STOPPING)
316 set_state(data, A2DP_STATE_CONFIGURED);
317 return err;
318 }
319
default_bitpool(uint8_t freq,uint8_t mode)320 static uint8_t default_bitpool(uint8_t freq, uint8_t mode)
321 {
322 switch (freq) {
323 case BT_SBC_SAMPLING_FREQ_16000:
324 case BT_SBC_SAMPLING_FREQ_32000:
325 return 53;
326 case BT_SBC_SAMPLING_FREQ_44100:
327 switch (mode) {
328 case BT_A2DP_CHANNEL_MODE_MONO:
329 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
330 return 31;
331 case BT_A2DP_CHANNEL_MODE_STEREO:
332 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
333 return 53;
334 default:
335 ERR("Invalid channel mode %u", mode);
336 return 53;
337 }
338 case BT_SBC_SAMPLING_FREQ_48000:
339 switch (mode) {
340 case BT_A2DP_CHANNEL_MODE_MONO:
341 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
342 return 29;
343 case BT_A2DP_CHANNEL_MODE_STEREO:
344 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
345 return 51;
346 default:
347 ERR("Invalid channel mode %u", mode);
348 return 51;
349 }
350 default:
351 ERR("Invalid sampling freq %u", freq);
352 return 53;
353 }
354 }
355
bluetooth_a2dp_init(struct bluetooth_data * data)356 static int bluetooth_a2dp_init(struct bluetooth_data *data)
357 {
358 sbc_capabilities_t *cap = &data->sbc_capabilities;
359 unsigned int max_bitpool, min_bitpool;
360 int dir;
361
362 switch (data->rate) {
363 case 48000:
364 cap->frequency = BT_SBC_SAMPLING_FREQ_48000;
365 break;
366 case 44100:
367 cap->frequency = BT_SBC_SAMPLING_FREQ_44100;
368 break;
369 case 32000:
370 cap->frequency = BT_SBC_SAMPLING_FREQ_32000;
371 break;
372 case 16000:
373 cap->frequency = BT_SBC_SAMPLING_FREQ_16000;
374 break;
375 default:
376 ERR("Rate %d not supported", data->rate);
377 return -1;
378 }
379
380 if (data->channels == 2) {
381 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
382 cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
383 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
384 cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
385 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
386 cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
387 } else {
388 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
389 cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
390 }
391
392 if (!cap->channel_mode) {
393 ERR("No supported channel modes");
394 return -1;
395 }
396
397 if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
398 cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
399 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
400 cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
401 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
402 cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
403 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
404 cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
405 else {
406 ERR("No supported block lengths");
407 return -1;
408 }
409
410 if (cap->subbands & BT_A2DP_SUBBANDS_8)
411 cap->subbands = BT_A2DP_SUBBANDS_8;
412 else if (cap->subbands & BT_A2DP_SUBBANDS_4)
413 cap->subbands = BT_A2DP_SUBBANDS_4;
414 else {
415 ERR("No supported subbands");
416 return -1;
417 }
418
419 if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
420 cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
421 else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
422 cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
423
424 min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool);
425 max_bitpool = MIN(default_bitpool(cap->frequency,
426 cap->channel_mode),
427 cap->max_bitpool);
428
429 cap->min_bitpool = min_bitpool;
430 cap->max_bitpool = max_bitpool;
431
432 return 0;
433 }
434
bluetooth_a2dp_setup(struct bluetooth_data * data)435 static void bluetooth_a2dp_setup(struct bluetooth_data *data)
436 {
437 sbc_capabilities_t active_capabilities = data->sbc_capabilities;
438
439 sbc_reinit(&data->sbc, 0);
440
441 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
442 data->sbc.frequency = SBC_FREQ_16000;
443
444 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
445 data->sbc.frequency = SBC_FREQ_32000;
446
447 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
448 data->sbc.frequency = SBC_FREQ_44100;
449
450 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
451 data->sbc.frequency = SBC_FREQ_48000;
452
453 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
454 data->sbc.mode = SBC_MODE_MONO;
455
456 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
457 data->sbc.mode = SBC_MODE_DUAL_CHANNEL;
458
459 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
460 data->sbc.mode = SBC_MODE_STEREO;
461
462 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
463 data->sbc.mode = SBC_MODE_JOINT_STEREO;
464
465 data->sbc.allocation = active_capabilities.allocation_method
466 == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR
467 : SBC_AM_LOUDNESS;
468
469 switch (active_capabilities.subbands) {
470 case BT_A2DP_SUBBANDS_4:
471 data->sbc.subbands = SBC_SB_4;
472 break;
473 case BT_A2DP_SUBBANDS_8:
474 data->sbc.subbands = SBC_SB_8;
475 break;
476 }
477
478 switch (active_capabilities.block_length) {
479 case BT_A2DP_BLOCK_LENGTH_4:
480 data->sbc.blocks = SBC_BLK_4;
481 break;
482 case BT_A2DP_BLOCK_LENGTH_8:
483 data->sbc.blocks = SBC_BLK_8;
484 break;
485 case BT_A2DP_BLOCK_LENGTH_12:
486 data->sbc.blocks = SBC_BLK_12;
487 break;
488 case BT_A2DP_BLOCK_LENGTH_16:
489 data->sbc.blocks = SBC_BLK_16;
490 break;
491 }
492
493 data->sbc.bitpool = active_capabilities.max_bitpool;
494 data->codesize = sbc_get_codesize(&data->sbc);
495 data->frame_duration = sbc_get_frame_duration(&data->sbc);
496 DBG("frame_duration: %d us", data->frame_duration);
497 }
498
bluetooth_a2dp_hw_params(struct bluetooth_data * data)499 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data)
500 {
501 char buf[BT_SUGGESTED_BUFFER_SIZE];
502 struct bt_open_req *open_req = (void *) buf;
503 struct bt_open_rsp *open_rsp = (void *) buf;
504 struct bt_set_configuration_req *setconf_req = (void*) buf;
505 struct bt_set_configuration_rsp *setconf_rsp = (void*) buf;
506 int err;
507
508 memset(open_req, 0, BT_SUGGESTED_BUFFER_SIZE);
509 open_req->h.type = BT_REQUEST;
510 open_req->h.name = BT_OPEN;
511 open_req->h.length = sizeof(*open_req);
512 strncpy(open_req->destination, data->address, 18);
513 open_req->seid = data->sbc_capabilities.capability.seid;
514 open_req->lock = BT_WRITE_LOCK;
515
516 err = audioservice_send(data, &open_req->h);
517 if (err < 0)
518 return err;
519
520 open_rsp->h.length = sizeof(*open_rsp);
521 err = audioservice_expect(data, &open_rsp->h, BT_OPEN);
522 if (err < 0)
523 return err;
524
525 err = bluetooth_a2dp_init(data);
526 if (err < 0)
527 return err;
528
529
530 memset(setconf_req, 0, BT_SUGGESTED_BUFFER_SIZE);
531 setconf_req->h.type = BT_REQUEST;
532 setconf_req->h.name = BT_SET_CONFIGURATION;
533 setconf_req->h.length = sizeof(*setconf_req);
534 memcpy(&setconf_req->codec, &data->sbc_capabilities,
535 sizeof(data->sbc_capabilities));
536
537 setconf_req->codec.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
538 setconf_req->codec.length = sizeof(data->sbc_capabilities);
539 setconf_req->h.length += setconf_req->codec.length - sizeof(setconf_req->codec);
540
541 DBG("bluetooth_a2dp_hw_params sending configuration:\n");
542 switch (data->sbc_capabilities.channel_mode) {
543 case BT_A2DP_CHANNEL_MODE_MONO:
544 DBG("\tchannel_mode: MONO\n");
545 break;
546 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
547 DBG("\tchannel_mode: DUAL CHANNEL\n");
548 break;
549 case BT_A2DP_CHANNEL_MODE_STEREO:
550 DBG("\tchannel_mode: STEREO\n");
551 break;
552 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
553 DBG("\tchannel_mode: JOINT STEREO\n");
554 break;
555 default:
556 DBG("\tchannel_mode: UNKNOWN (%d)\n",
557 data->sbc_capabilities.channel_mode);
558 }
559 switch (data->sbc_capabilities.frequency) {
560 case BT_SBC_SAMPLING_FREQ_16000:
561 DBG("\tfrequency: 16000\n");
562 break;
563 case BT_SBC_SAMPLING_FREQ_32000:
564 DBG("\tfrequency: 32000\n");
565 break;
566 case BT_SBC_SAMPLING_FREQ_44100:
567 DBG("\tfrequency: 44100\n");
568 break;
569 case BT_SBC_SAMPLING_FREQ_48000:
570 DBG("\tfrequency: 48000\n");
571 break;
572 default:
573 DBG("\tfrequency: UNKNOWN (%d)\n",
574 data->sbc_capabilities.frequency);
575 }
576 switch (data->sbc_capabilities.allocation_method) {
577 case BT_A2DP_ALLOCATION_SNR:
578 DBG("\tallocation_method: SNR\n");
579 break;
580 case BT_A2DP_ALLOCATION_LOUDNESS:
581 DBG("\tallocation_method: LOUDNESS\n");
582 break;
583 default:
584 DBG("\tallocation_method: UNKNOWN (%d)\n",
585 data->sbc_capabilities.allocation_method);
586 }
587 switch (data->sbc_capabilities.subbands) {
588 case BT_A2DP_SUBBANDS_4:
589 DBG("\tsubbands: 4\n");
590 break;
591 case BT_A2DP_SUBBANDS_8:
592 DBG("\tsubbands: 8\n");
593 break;
594 default:
595 DBG("\tsubbands: UNKNOWN (%d)\n",
596 data->sbc_capabilities.subbands);
597 }
598 switch (data->sbc_capabilities.block_length) {
599 case BT_A2DP_BLOCK_LENGTH_4:
600 DBG("\tblock_length: 4\n");
601 break;
602 case BT_A2DP_BLOCK_LENGTH_8:
603 DBG("\tblock_length: 8\n");
604 break;
605 case BT_A2DP_BLOCK_LENGTH_12:
606 DBG("\tblock_length: 12\n");
607 break;
608 case BT_A2DP_BLOCK_LENGTH_16:
609 DBG("\tblock_length: 16\n");
610 break;
611 default:
612 DBG("\tblock_length: UNKNOWN (%d)\n",
613 data->sbc_capabilities.block_length);
614 }
615 DBG("\tmin_bitpool: %d\n", data->sbc_capabilities.min_bitpool);
616 DBG("\tmax_bitpool: %d\n", data->sbc_capabilities.max_bitpool);
617
618 err = audioservice_send(data, &setconf_req->h);
619 if (err < 0)
620 return err;
621
622 err = audioservice_expect(data, &setconf_rsp->h, BT_SET_CONFIGURATION);
623 if (err < 0)
624 return err;
625
626 data->link_mtu = setconf_rsp->link_mtu;
627 DBG("MTU: %d", data->link_mtu);
628
629 /* Setup SBC encoder now we agree on parameters */
630 bluetooth_a2dp_setup(data);
631
632 DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
633 data->sbc.allocation, data->sbc.subbands, data->sbc.blocks,
634 data->sbc.bitpool);
635
636 return 0;
637 }
638
avdtp_write(struct bluetooth_data * data)639 static int avdtp_write(struct bluetooth_data *data)
640 {
641 int ret = 0;
642 struct rtp_header *header;
643 struct rtp_payload *payload;
644
645 uint64_t now;
646 long duration = data->frame_duration * data->frame_count;
647 #ifdef ENABLE_TIMING
648 uint64_t begin, end, begin2, end2;
649 begin = get_microseconds();
650 #endif
651
652 header = (struct rtp_header *)data->buffer;
653 payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
654
655 memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
656
657 payload->frame_count = data->frame_count;
658 header->v = 2;
659 header->pt = 1;
660 header->sequence_number = htons(data->seq_num);
661 header->timestamp = htonl(data->nsamples);
662 header->ssrc = htonl(1);
663
664 data->stream.revents = 0;
665 #ifdef ENABLE_TIMING
666 begin2 = get_microseconds();
667 #endif
668 ret = poll(&data->stream, 1, POLL_TIMEOUT);
669 #ifdef ENABLE_TIMING
670 end2 = get_microseconds();
671 print_time("poll", begin2, end2);
672 #endif
673 if (ret == 1 && data->stream.revents == POLLOUT) {
674 long ahead = 0;
675 now = get_microseconds();
676
677 if (data->next_write) {
678 ahead = data->next_write - now;
679 #ifdef ENABLE_TIMING
680 DBG("duration: %ld, ahead: %ld", duration, ahead);
681 #endif
682 if (ahead > 0) {
683 /* too fast, need to throttle */
684 usleep(ahead);
685 }
686 } else {
687 data->next_write = now;
688 }
689 if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
690 /* fallen too far behind, don't try to catch up */
691 VDBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000);
692 data->next_write = 0;
693 } else {
694 data->next_write += duration;
695 }
696
697 #ifdef ENABLE_TIMING
698 begin2 = get_microseconds();
699 #endif
700 ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
701 #ifdef ENABLE_TIMING
702 end2 = get_microseconds();
703 print_time("send", begin2, end2);
704 #endif
705 if (ret < 0) {
706 /* can happen during normal remote disconnect */
707 VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
708 }
709 if (ret == -EPIPE) {
710 bluetooth_close(data);
711 }
712 } else {
713 /* can happen during normal remote disconnect */
714 VDBG("poll() failed: %d (revents = %d, errno %s)",
715 ret, data->stream.revents, strerror(errno));
716 data->next_write = 0;
717 }
718
719 /* Reset buffer of data to send */
720 data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
721 data->frame_count = 0;
722 data->samples = 0;
723 data->seq_num++;
724
725 #ifdef ENABLE_TIMING
726 end = get_microseconds();
727 print_time("avdtp_write", begin, end);
728 #endif
729 return 0; /* always return success */
730 }
731
audioservice_send(struct bluetooth_data * data,const bt_audio_msg_header_t * msg)732 static int audioservice_send(struct bluetooth_data *data,
733 const bt_audio_msg_header_t *msg)
734 {
735 int err;
736 uint16_t length;
737
738 length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
739
740 VDBG("sending %s", bt_audio_strtype(msg->type));
741 if (send(data->server.fd, msg, length,
742 MSG_NOSIGNAL) > 0)
743 err = 0;
744 else {
745 err = -errno;
746 ERR("Error sending data to audio service: %s(%d)",
747 strerror(errno), errno);
748 if (err == -EPIPE)
749 bluetooth_close(data);
750 }
751
752 return err;
753 }
754
audioservice_recv(struct bluetooth_data * data,bt_audio_msg_header_t * inmsg)755 static int audioservice_recv(struct bluetooth_data *data,
756 bt_audio_msg_header_t *inmsg)
757 {
758 int err, ret;
759 const char *type, *name;
760 uint16_t length;
761
762 length = inmsg->length ? inmsg->length : BT_SUGGESTED_BUFFER_SIZE;
763
764 ret = recv(data->server.fd, inmsg, length, 0);
765 if (ret < 0) {
766 err = -errno;
767 ERR("Error receiving IPC data from bluetoothd: %s (%d)",
768 strerror(errno), errno);
769 if (err == -EPIPE)
770 bluetooth_close(data);
771 } else if ((size_t) ret < sizeof(bt_audio_msg_header_t)) {
772 ERR("Too short (%d bytes) IPC packet from bluetoothd", ret);
773 err = -EINVAL;
774 } else if (inmsg->type == BT_ERROR) {
775 bt_audio_error_t *error = (bt_audio_error_t *)inmsg;
776 ret = recv(data->server.fd, &error->posix_errno,
777 sizeof(error->posix_errno), 0);
778 if (ret < 0) {
779 err = -errno;
780 ERR("Error receiving error code for BT_ERROR: %s (%d)",
781 strerror(errno), errno);
782 if (err == -EPIPE)
783 bluetooth_close(data);
784 } else {
785 ERR("%s failed : %s(%d)",
786 bt_audio_strname(error->h.name),
787 strerror(error->posix_errno),
788 error->posix_errno);
789 err = -error->posix_errno;
790 }
791 } else {
792 type = bt_audio_strtype(inmsg->type);
793 name = bt_audio_strname(inmsg->name);
794 if (type && name) {
795 DBG("Received %s - %s", type, name);
796 err = 0;
797 } else {
798 err = -EINVAL;
799 ERR("Bogus message type %d - name %d"
800 " received from audio service",
801 inmsg->type, inmsg->name);
802 }
803
804 }
805 return err;
806 }
807
audioservice_expect(struct bluetooth_data * data,bt_audio_msg_header_t * rsp_hdr,int expected_name)808 static int audioservice_expect(struct bluetooth_data *data,
809 bt_audio_msg_header_t *rsp_hdr, int expected_name)
810 {
811 int err = audioservice_recv(data, rsp_hdr);
812
813 if (err != 0)
814 return err;
815
816 if (rsp_hdr->name != expected_name) {
817 err = -EINVAL;
818 ERR("Bogus message %s received while %s was expected",
819 bt_audio_strname(rsp_hdr->name),
820 bt_audio_strname(expected_name));
821 }
822 return err;
823
824 }
825
bluetooth_init(struct bluetooth_data * data)826 static int bluetooth_init(struct bluetooth_data *data)
827 {
828 int sk, err;
829 struct timeval tv = {.tv_sec = RECV_TIMEOUT};
830
831 DBG("bluetooth_init");
832
833 sk = bt_audio_service_open();
834 if (sk < 0) {
835 ERR("bt_audio_service_open failed\n");
836 return -errno;
837 }
838
839 err = setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
840 if (err < 0) {
841 ERR("bluetooth_init setsockopt(SO_RCVTIMEO) failed %d", err);
842 return err;
843 }
844
845 data->server.fd = sk;
846 data->server.events = POLLIN;
847 data->state = A2DP_STATE_INITIALIZED;
848
849 return 0;
850 }
851
bluetooth_parse_capabilities(struct bluetooth_data * data,struct bt_get_capabilities_rsp * rsp)852 static int bluetooth_parse_capabilities(struct bluetooth_data *data,
853 struct bt_get_capabilities_rsp *rsp)
854 {
855 int bytes_left = rsp->h.length - sizeof(*rsp);
856 codec_capabilities_t *codec = (void *) rsp->data;
857
858 if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP)
859 return -EINVAL;
860
861 while (bytes_left > 0) {
862 if ((codec->type == BT_A2DP_SBC_SINK) &&
863 !(codec->lock & BT_WRITE_LOCK))
864 break;
865
866 if (codec->length == 0) {
867 ERR("bluetooth_parse_capabilities() invalid codec capabilities length");
868 return -EINVAL;
869 }
870 bytes_left -= codec->length;
871 codec = (codec_capabilities_t *)((char *)codec + codec->length);
872 }
873
874 if (bytes_left <= 0 ||
875 codec->length != sizeof(data->sbc_capabilities))
876 return -EINVAL;
877
878 memcpy(&data->sbc_capabilities, codec, codec->length);
879 return 0;
880 }
881
882
bluetooth_configure(struct bluetooth_data * data)883 static int bluetooth_configure(struct bluetooth_data *data)
884 {
885 char buf[BT_SUGGESTED_BUFFER_SIZE];
886 struct bt_get_capabilities_req *getcaps_req = (void*) buf;
887 struct bt_get_capabilities_rsp *getcaps_rsp = (void*) buf;
888 int err;
889
890 DBG("bluetooth_configure");
891
892 data->state = A2DP_STATE_CONFIGURING;
893 memset(getcaps_req, 0, BT_SUGGESTED_BUFFER_SIZE);
894 getcaps_req->h.type = BT_REQUEST;
895 getcaps_req->h.name = BT_GET_CAPABILITIES;
896
897 getcaps_req->flags = 0;
898 getcaps_req->flags |= BT_FLAG_AUTOCONNECT;
899 strncpy(getcaps_req->destination, data->address, 18);
900 getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
901 getcaps_req->h.length = sizeof(*getcaps_req);
902
903 err = audioservice_send(data, &getcaps_req->h);
904 if (err < 0) {
905 ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n");
906 goto error;
907 }
908
909 getcaps_rsp->h.length = 0;
910 err = audioservice_expect(data, &getcaps_rsp->h, BT_GET_CAPABILITIES);
911 if (err < 0) {
912 ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n");
913 goto error;
914 }
915
916 err = bluetooth_parse_capabilities(data, getcaps_rsp);
917 if (err < 0) {
918 ERR("bluetooth_parse_capabilities failed err: %d", err);
919 goto error;
920 }
921
922 err = bluetooth_a2dp_hw_params(data);
923 if (err < 0) {
924 ERR("bluetooth_a2dp_hw_params failed err: %d", err);
925 goto error;
926 }
927
928
929 set_state(data, A2DP_STATE_CONFIGURED);
930 return 0;
931
932 error:
933
934 if (data->state == A2DP_STATE_CONFIGURING) {
935 bluetooth_close(data);
936 /* notify client that thread is ready for next command */
937 pthread_cond_signal(&data->client_wait);
938 }
939 return err;
940 }
941
set_state(struct bluetooth_data * data,a2dp_state_t state)942 static void set_state(struct bluetooth_data *data, a2dp_state_t state)
943 {
944 data->state = state;
945 pthread_cond_signal(&data->client_wait);
946 }
947
__set_command(struct bluetooth_data * data,a2dp_command_t command)948 static void __set_command(struct bluetooth_data *data, a2dp_command_t command)
949 {
950 VDBG("set_command %d\n", command);
951 data->command = command;
952 pthread_cond_signal(&data->thread_wait);
953 return;
954 }
955
set_command(struct bluetooth_data * data,a2dp_command_t command)956 static void set_command(struct bluetooth_data *data, a2dp_command_t command)
957 {
958 pthread_mutex_lock(&data->mutex);
959 __set_command(data, command);
960 pthread_mutex_unlock(&data->mutex);
961 }
962
963 /* timeout is in milliseconds */
wait_for_start(struct bluetooth_data * data,int timeout)964 static int wait_for_start(struct bluetooth_data *data, int timeout)
965 {
966 a2dp_state_t state = data->state;
967 struct timeval tv;
968 struct timespec ts;
969 int err = 0;
970
971 #ifdef ENABLE_TIMING
972 uint64_t begin, end;
973 begin = get_microseconds();
974 #endif
975
976 gettimeofday(&tv, (struct timezone *) NULL);
977 ts.tv_sec = tv.tv_sec + (timeout / 1000);
978 ts.tv_nsec = (tv.tv_usec + (timeout % 1000) * 1000L ) * 1000L;
979
980 pthread_mutex_lock(&data->mutex);
981 while (state != A2DP_STATE_STARTED) {
982 if (state == A2DP_STATE_NONE)
983 __set_command(data, A2DP_CMD_INIT);
984 else if (state == A2DP_STATE_INITIALIZED)
985 __set_command(data, A2DP_CMD_CONFIGURE);
986 else if (state == A2DP_STATE_CONFIGURED) {
987 __set_command(data, A2DP_CMD_START);
988 }
989 again:
990 err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts);
991 if (err) {
992 /* don't timeout if we're done */
993 if (data->state == A2DP_STATE_STARTED) {
994 err = 0;
995 break;
996 }
997 if (err == ETIMEDOUT)
998 break;
999 goto again;
1000 }
1001
1002 if (state == data->state)
1003 goto again;
1004
1005 state = data->state;
1006
1007 if (state == A2DP_STATE_NONE) {
1008 err = ENODEV;
1009 break;
1010 }
1011 }
1012 pthread_mutex_unlock(&data->mutex);
1013
1014 #ifdef ENABLE_TIMING
1015 end = get_microseconds();
1016 print_time("wait_for_start", begin, end);
1017 #endif
1018
1019 /* pthread_cond_timedwait returns positive errors */
1020 return -err;
1021 }
1022
a2dp_free(struct bluetooth_data * data)1023 static void a2dp_free(struct bluetooth_data *data)
1024 {
1025 pthread_cond_destroy(&data->client_wait);
1026 pthread_cond_destroy(&data->thread_wait);
1027 pthread_cond_destroy(&data->thread_start);
1028 pthread_mutex_destroy(&data->mutex);
1029 free(data);
1030 return;
1031 }
1032
a2dp_thread(void * d)1033 static void* a2dp_thread(void *d)
1034 {
1035 struct bluetooth_data* data = (struct bluetooth_data*)d;
1036 a2dp_command_t command = A2DP_CMD_NONE;
1037 int err = 0;
1038
1039 DBG("a2dp_thread started");
1040 prctl(PR_SET_NAME, (int)"a2dp_thread", 0, 0, 0);
1041
1042 pthread_mutex_lock(&data->mutex);
1043
1044 data->started = 1;
1045 pthread_cond_signal(&data->thread_start);
1046
1047 while (1)
1048 {
1049 while (1) {
1050 pthread_cond_wait(&data->thread_wait, &data->mutex);
1051
1052 /* Initialization needed */
1053 if (data->state == A2DP_STATE_NONE &&
1054 data->command != A2DP_CMD_QUIT) {
1055 err = bluetooth_init(data);
1056 }
1057
1058 /* New state command signaled */
1059 if (command != data->command) {
1060 command = data->command;
1061 break;
1062 }
1063 }
1064
1065 switch (command) {
1066 case A2DP_CMD_CONFIGURE:
1067 if (data->state != A2DP_STATE_INITIALIZED)
1068 break;
1069 err = bluetooth_configure(data);
1070 break;
1071
1072 case A2DP_CMD_START:
1073 if (data->state != A2DP_STATE_CONFIGURED)
1074 break;
1075 err = bluetooth_start(data);
1076 break;
1077
1078 case A2DP_CMD_STOP:
1079 if (data->state != A2DP_STATE_STARTED)
1080 break;
1081 err = bluetooth_stop(data);
1082 break;
1083
1084 case A2DP_CMD_QUIT:
1085 bluetooth_close(data);
1086 sbc_finish(&data->sbc);
1087 a2dp_free(data);
1088 goto done;
1089
1090 case A2DP_CMD_INIT:
1091 /* already called bluetooth_init() */
1092 default:
1093 break;
1094 }
1095 // reset last command in case of error to allow
1096 // re-execution of the same command
1097 if (err < 0) {
1098 command = A2DP_CMD_NONE;
1099 }
1100 }
1101
1102 done:
1103 pthread_mutex_unlock(&data->mutex);
1104 DBG("a2dp_thread finished");
1105 return NULL;
1106 }
1107
a2dp_init(int rate,int channels,a2dpData * dataPtr)1108 int a2dp_init(int rate, int channels, a2dpData* dataPtr)
1109 {
1110 struct bluetooth_data* data;
1111 pthread_attr_t attr;
1112 int err;
1113
1114 DBG("a2dp_init rate: %d channels: %d", rate, channels);
1115 *dataPtr = NULL;
1116 data = malloc(sizeof(struct bluetooth_data));
1117 if (!data)
1118 return -1;
1119
1120 memset(data, 0, sizeof(struct bluetooth_data));
1121 data->server.fd = -1;
1122 data->stream.fd = -1;
1123 data->state = A2DP_STATE_NONE;
1124 data->command = A2DP_CMD_NONE;
1125
1126 strncpy(data->address, "00:00:00:00:00:00", 18);
1127 data->rate = rate;
1128 data->channels = channels;
1129
1130 sbc_init(&data->sbc, 0);
1131
1132 pthread_mutex_init(&data->mutex, NULL);
1133 pthread_cond_init(&data->thread_start, NULL);
1134 pthread_cond_init(&data->thread_wait, NULL);
1135 pthread_cond_init(&data->client_wait, NULL);
1136
1137 pthread_mutex_lock(&data->mutex);
1138 data->started = 0;
1139
1140 pthread_attr_init(&attr);
1141 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
1142
1143 err = pthread_create(&data->thread, &attr, a2dp_thread, data);
1144 if (err) {
1145 /* If the thread create fails we must not wait */
1146 pthread_mutex_unlock(&data->mutex);
1147 err = -err;
1148 goto error;
1149 }
1150
1151 /* Make sure the state machine is ready and waiting */
1152 while (!data->started) {
1153 pthread_cond_wait(&data->thread_start, &data->mutex);
1154 }
1155
1156 /* Poke the state machine to get it going */
1157 pthread_cond_signal(&data->thread_wait);
1158
1159 pthread_mutex_unlock(&data->mutex);
1160 pthread_attr_destroy(&attr);
1161
1162 *dataPtr = data;
1163 return 0;
1164 error:
1165 bluetooth_close(data);
1166 sbc_finish(&data->sbc);
1167 pthread_attr_destroy(&attr);
1168 a2dp_free(data);
1169
1170 return err;
1171 }
1172
a2dp_set_sink(a2dpData d,const char * address)1173 void a2dp_set_sink(a2dpData d, const char* address)
1174 {
1175 struct bluetooth_data* data = (struct bluetooth_data*)d;
1176 if (strncmp(data->address, address, 18)) {
1177 strncpy(data->address, address, 18);
1178 set_command(data, A2DP_CMD_INIT);
1179 }
1180 }
1181
a2dp_write(a2dpData d,const void * buffer,int count)1182 int a2dp_write(a2dpData d, const void* buffer, int count)
1183 {
1184 struct bluetooth_data* data = (struct bluetooth_data*)d;
1185 uint8_t* src = (uint8_t *)buffer;
1186 int codesize;
1187 int err, ret = 0;
1188 long frames_left = count;
1189 int encoded;
1190 unsigned int written;
1191 const char *buff;
1192 int did_configure = 0;
1193 #ifdef ENABLE_TIMING
1194 uint64_t begin, end;
1195 DBG("********** a2dp_write **********");
1196 begin = get_microseconds();
1197 #endif
1198
1199 err = wait_for_start(data, WRITE_TIMEOUT);
1200 if (err < 0)
1201 return err;
1202
1203 codesize = data->codesize;
1204
1205 while (frames_left >= codesize) {
1206 /* Enough data to encode (sbc wants 512 byte blocks) */
1207 encoded = sbc_encode(&(data->sbc), src, codesize,
1208 data->buffer + data->count,
1209 sizeof(data->buffer) - data->count,
1210 &written);
1211 if (encoded <= 0) {
1212 ERR("Encoding error %d", encoded);
1213 goto done;
1214 }
1215 VDBG("sbc_encode returned %d, codesize: %d, written: %d\n",
1216 encoded, codesize, written);
1217
1218 src += encoded;
1219 data->count += written;
1220 data->frame_count++;
1221 data->samples += encoded;
1222 data->nsamples += encoded;
1223
1224 /* No space left for another frame then send */
1225 if ((data->count + written >= data->link_mtu) ||
1226 (data->count + written >= BUFFER_SIZE)) {
1227 VDBG("sending packet %d, count %d, link_mtu %u",
1228 data->seq_num, data->count,
1229 data->link_mtu);
1230 err = avdtp_write(data);
1231 if (err < 0)
1232 return err;
1233 }
1234
1235 ret += encoded;
1236 frames_left -= encoded;
1237 }
1238
1239 if (frames_left > 0)
1240 ERR("%ld bytes left at end of a2dp_write\n", frames_left);
1241
1242 done:
1243 #ifdef ENABLE_TIMING
1244 end = get_microseconds();
1245 print_time("a2dp_write total", begin, end);
1246 #endif
1247 return ret;
1248 }
1249
a2dp_stop(a2dpData d)1250 int a2dp_stop(a2dpData d)
1251 {
1252 struct bluetooth_data* data = (struct bluetooth_data*)d;
1253 DBG("a2dp_stop\n");
1254 if (!data)
1255 return 0;
1256
1257 set_command(data, A2DP_CMD_STOP);
1258 return 0;
1259 }
1260
a2dp_cleanup(a2dpData d)1261 void a2dp_cleanup(a2dpData d)
1262 {
1263 struct bluetooth_data* data = (struct bluetooth_data*)d;
1264 DBG("a2dp_cleanup\n");
1265 set_command(data, A2DP_CMD_QUIT);
1266 }
1267