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 (at) 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 ALOGD 62 #else 63 #define DBG(fmt, arg...) 64 #endif 65 66 #ifdef ENABLE_VERBOSE 67 #define VDBG ALOGV 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 ALOGE 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 */ 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 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 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 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 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 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 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 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
