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