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 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 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_L2CAP, L2CAP_LM, &flags, &len) < 0) 198 return -errno; 199 200 if (flushable) { 201 if (flags & L2CAP_LM_FLUSHABLE) 202 return 0; 203 flags |= L2CAP_LM_FLUSHABLE; 204 } else { 205 if (!(flags & L2CAP_LM_FLUSHABLE)) 206 return 0; 207 flags &= ~L2CAP_LM_FLUSHABLE; 208 } 209 210 if (setsockopt(fd, SOL_L2CAP, 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 return err; 278 } 279 280 static int bluetooth_stop(struct bluetooth_data *data) 281 { 282 char buf[BT_SUGGESTED_BUFFER_SIZE]; 283 struct bt_stop_stream_req *stop_req = (void*) buf; 284 struct bt_stop_stream_rsp *stop_rsp = (void*) buf; 285 int err; 286 287 DBG("bluetooth_stop"); 288 289 data->state = A2DP_STATE_STOPPING; 290 l2cap_set_flushable(data->stream.fd, 0); 291 if (data->stream.fd >= 0) { 292 close(data->stream.fd); 293 data->stream.fd = -1; 294 } 295 296 /* send stop request */ 297 memset(stop_req, 0, BT_SUGGESTED_BUFFER_SIZE); 298 stop_req->h.type = BT_REQUEST; 299 stop_req->h.name = BT_STOP_STREAM; 300 stop_req->h.length = sizeof(*stop_req); 301 302 err = audioservice_send(data, &stop_req->h); 303 if (err < 0) 304 goto error; 305 306 stop_rsp->h.length = sizeof(*stop_rsp); 307 err = audioservice_expect(data, &stop_rsp->h, BT_STOP_STREAM); 308 if (err < 0) 309 goto error; 310 311 error: 312 if (data->state == A2DP_STATE_STOPPING) 313 set_state(data, A2DP_STATE_CONFIGURED); 314 return err; 315 } 316 317 static uint8_t default_bitpool(uint8_t freq, uint8_t mode) 318 { 319 switch (freq) { 320 case BT_SBC_SAMPLING_FREQ_16000: 321 case BT_SBC_SAMPLING_FREQ_32000: 322 return 53; 323 case BT_SBC_SAMPLING_FREQ_44100: 324 switch (mode) { 325 case BT_A2DP_CHANNEL_MODE_MONO: 326 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 327 return 31; 328 case BT_A2DP_CHANNEL_MODE_STEREO: 329 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 330 return 53; 331 default: 332 ERR("Invalid channel mode %u", mode); 333 return 53; 334 } 335 case BT_SBC_SAMPLING_FREQ_48000: 336 switch (mode) { 337 case BT_A2DP_CHANNEL_MODE_MONO: 338 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 339 return 29; 340 case BT_A2DP_CHANNEL_MODE_STEREO: 341 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 342 return 51; 343 default: 344 ERR("Invalid channel mode %u", mode); 345 return 51; 346 } 347 default: 348 ERR("Invalid sampling freq %u", freq); 349 return 53; 350 } 351 } 352 353 static int bluetooth_a2dp_init(struct bluetooth_data *data) 354 { 355 sbc_capabilities_t *cap = &data->sbc_capabilities; 356 unsigned int max_bitpool, min_bitpool; 357 int dir; 358 359 switch (data->rate) { 360 case 48000: 361 cap->frequency = BT_SBC_SAMPLING_FREQ_48000; 362 break; 363 case 44100: 364 cap->frequency = BT_SBC_SAMPLING_FREQ_44100; 365 break; 366 case 32000: 367 cap->frequency = BT_SBC_SAMPLING_FREQ_32000; 368 break; 369 case 16000: 370 cap->frequency = BT_SBC_SAMPLING_FREQ_16000; 371 break; 372 default: 373 ERR("Rate %d not supported", data->rate); 374 return -1; 375 } 376 377 if (data->channels == 2) { 378 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO) 379 cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO; 380 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO) 381 cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO; 382 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL) 383 cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL; 384 } else { 385 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) 386 cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO; 387 } 388 389 if (!cap->channel_mode) { 390 ERR("No supported channel modes"); 391 return -1; 392 } 393 394 if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16) 395 cap->block_length = BT_A2DP_BLOCK_LENGTH_16; 396 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12) 397 cap->block_length = BT_A2DP_BLOCK_LENGTH_12; 398 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8) 399 cap->block_length = BT_A2DP_BLOCK_LENGTH_8; 400 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4) 401 cap->block_length = BT_A2DP_BLOCK_LENGTH_4; 402 else { 403 ERR("No supported block lengths"); 404 return -1; 405 } 406 407 if (cap->subbands & BT_A2DP_SUBBANDS_8) 408 cap->subbands = BT_A2DP_SUBBANDS_8; 409 else if (cap->subbands & BT_A2DP_SUBBANDS_4) 410 cap->subbands = BT_A2DP_SUBBANDS_4; 411 else { 412 ERR("No supported subbands"); 413 return -1; 414 } 415 416 if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS) 417 cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS; 418 else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR) 419 cap->allocation_method = BT_A2DP_ALLOCATION_SNR; 420 421 min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool); 422 max_bitpool = MIN(default_bitpool(cap->frequency, 423 cap->channel_mode), 424 cap->max_bitpool); 425 426 cap->min_bitpool = min_bitpool; 427 cap->max_bitpool = max_bitpool; 428 429 return 0; 430 } 431 432 static void bluetooth_a2dp_setup(struct bluetooth_data *data) 433 { 434 sbc_capabilities_t active_capabilities = data->sbc_capabilities; 435 436 sbc_reinit(&data->sbc, 0); 437 438 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000) 439 data->sbc.frequency = SBC_FREQ_16000; 440 441 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000) 442 data->sbc.frequency = SBC_FREQ_32000; 443 444 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100) 445 data->sbc.frequency = SBC_FREQ_44100; 446 447 if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000) 448 data->sbc.frequency = SBC_FREQ_48000; 449 450 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO) 451 data->sbc.mode = SBC_MODE_MONO; 452 453 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL) 454 data->sbc.mode = SBC_MODE_DUAL_CHANNEL; 455 456 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO) 457 data->sbc.mode = SBC_MODE_STEREO; 458 459 if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO) 460 data->sbc.mode = SBC_MODE_JOINT_STEREO; 461 462 data->sbc.allocation = active_capabilities.allocation_method 463 == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR 464 : SBC_AM_LOUDNESS; 465 466 switch (active_capabilities.subbands) { 467 case BT_A2DP_SUBBANDS_4: 468 data->sbc.subbands = SBC_SB_4; 469 break; 470 case BT_A2DP_SUBBANDS_8: 471 data->sbc.subbands = SBC_SB_8; 472 break; 473 } 474 475 switch (active_capabilities.block_length) { 476 case BT_A2DP_BLOCK_LENGTH_4: 477 data->sbc.blocks = SBC_BLK_4; 478 break; 479 case BT_A2DP_BLOCK_LENGTH_8: 480 data->sbc.blocks = SBC_BLK_8; 481 break; 482 case BT_A2DP_BLOCK_LENGTH_12: 483 data->sbc.blocks = SBC_BLK_12; 484 break; 485 case BT_A2DP_BLOCK_LENGTH_16: 486 data->sbc.blocks = SBC_BLK_16; 487 break; 488 } 489 490 data->sbc.bitpool = active_capabilities.max_bitpool; 491 data->codesize = sbc_get_codesize(&data->sbc); 492 data->frame_duration = sbc_get_frame_duration(&data->sbc); 493 DBG("frame_duration: %d us", data->frame_duration); 494 } 495 496 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data) 497 { 498 char buf[BT_SUGGESTED_BUFFER_SIZE]; 499 struct bt_open_req *open_req = (void *) buf; 500 struct bt_open_rsp *open_rsp = (void *) buf; 501 struct bt_set_configuration_req *setconf_req = (void*) buf; 502 struct bt_set_configuration_rsp *setconf_rsp = (void*) buf; 503 int err; 504 505 memset(open_req, 0, BT_SUGGESTED_BUFFER_SIZE); 506 open_req->h.type = BT_REQUEST; 507 open_req->h.name = BT_OPEN; 508 open_req->h.length = sizeof(*open_req); 509 strncpy(open_req->destination, data->address, 18); 510 open_req->seid = data->sbc_capabilities.capability.seid; 511 open_req->lock = BT_WRITE_LOCK; 512 513 err = audioservice_send(data, &open_req->h); 514 if (err < 0) 515 return err; 516 517 open_rsp->h.length = sizeof(*open_rsp); 518 err = audioservice_expect(data, &open_rsp->h, BT_OPEN); 519 if (err < 0) 520 return err; 521 522 err = bluetooth_a2dp_init(data); 523 if (err < 0) 524 return err; 525 526 527 memset(setconf_req, 0, BT_SUGGESTED_BUFFER_SIZE); 528 setconf_req->h.type = BT_REQUEST; 529 setconf_req->h.name = BT_SET_CONFIGURATION; 530 setconf_req->h.length = sizeof(*setconf_req); 531 memcpy(&setconf_req->codec, &data->sbc_capabilities, 532 sizeof(data->sbc_capabilities)); 533 534 setconf_req->codec.transport = BT_CAPABILITIES_TRANSPORT_A2DP; 535 setconf_req->codec.length = sizeof(data->sbc_capabilities); 536 setconf_req->h.length += setconf_req->codec.length - sizeof(setconf_req->codec); 537 538 DBG("bluetooth_a2dp_hw_params sending configuration:\n"); 539 switch (data->sbc_capabilities.channel_mode) { 540 case BT_A2DP_CHANNEL_MODE_MONO: 541 DBG("\tchannel_mode: MONO\n"); 542 break; 543 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 544 DBG("\tchannel_mode: DUAL CHANNEL\n"); 545 break; 546 case BT_A2DP_CHANNEL_MODE_STEREO: 547 DBG("\tchannel_mode: STEREO\n"); 548 break; 549 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 550 DBG("\tchannel_mode: JOINT STEREO\n"); 551 break; 552 default: 553 DBG("\tchannel_mode: UNKNOWN (%d)\n", 554 data->sbc_capabilities.channel_mode); 555 } 556 switch (data->sbc_capabilities.frequency) { 557 case BT_SBC_SAMPLING_FREQ_16000: 558 DBG("\tfrequency: 16000\n"); 559 break; 560 case BT_SBC_SAMPLING_FREQ_32000: 561 DBG("\tfrequency: 32000\n"); 562 break; 563 case BT_SBC_SAMPLING_FREQ_44100: 564 DBG("\tfrequency: 44100\n"); 565 break; 566 case BT_SBC_SAMPLING_FREQ_48000: 567 DBG("\tfrequency: 48000\n"); 568 break; 569 default: 570 DBG("\tfrequency: UNKNOWN (%d)\n", 571 data->sbc_capabilities.frequency); 572 } 573 switch (data->sbc_capabilities.allocation_method) { 574 case BT_A2DP_ALLOCATION_SNR: 575 DBG("\tallocation_method: SNR\n"); 576 break; 577 case BT_A2DP_ALLOCATION_LOUDNESS: 578 DBG("\tallocation_method: LOUDNESS\n"); 579 break; 580 default: 581 DBG("\tallocation_method: UNKNOWN (%d)\n", 582 data->sbc_capabilities.allocation_method); 583 } 584 switch (data->sbc_capabilities.subbands) { 585 case BT_A2DP_SUBBANDS_4: 586 DBG("\tsubbands: 4\n"); 587 break; 588 case BT_A2DP_SUBBANDS_8: 589 DBG("\tsubbands: 8\n"); 590 break; 591 default: 592 DBG("\tsubbands: UNKNOWN (%d)\n", 593 data->sbc_capabilities.subbands); 594 } 595 switch (data->sbc_capabilities.block_length) { 596 case BT_A2DP_BLOCK_LENGTH_4: 597 DBG("\tblock_length: 4\n"); 598 break; 599 case BT_A2DP_BLOCK_LENGTH_8: 600 DBG("\tblock_length: 8\n"); 601 break; 602 case BT_A2DP_BLOCK_LENGTH_12: 603 DBG("\tblock_length: 12\n"); 604 break; 605 case BT_A2DP_BLOCK_LENGTH_16: 606 DBG("\tblock_length: 16\n"); 607 break; 608 default: 609 DBG("\tblock_length: UNKNOWN (%d)\n", 610 data->sbc_capabilities.block_length); 611 } 612 DBG("\tmin_bitpool: %d\n", data->sbc_capabilities.min_bitpool); 613 DBG("\tmax_bitpool: %d\n", data->sbc_capabilities.max_bitpool); 614 615 err = audioservice_send(data, &setconf_req->h); 616 if (err < 0) 617 return err; 618 619 err = audioservice_expect(data, &setconf_rsp->h, BT_SET_CONFIGURATION); 620 if (err < 0) 621 return err; 622 623 data->link_mtu = setconf_rsp->link_mtu; 624 DBG("MTU: %d", data->link_mtu); 625 626 /* Setup SBC encoder now we agree on parameters */ 627 bluetooth_a2dp_setup(data); 628 629 DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n", 630 data->sbc.allocation, data->sbc.subbands, data->sbc.blocks, 631 data->sbc.bitpool); 632 633 return 0; 634 } 635 636 static int avdtp_write(struct bluetooth_data *data) 637 { 638 int ret = 0; 639 struct rtp_header *header; 640 struct rtp_payload *payload; 641 642 uint64_t now; 643 long duration = data->frame_duration * data->frame_count; 644 #ifdef ENABLE_TIMING 645 uint64_t begin, end, begin2, end2; 646 begin = get_microseconds(); 647 #endif 648 649 header = (struct rtp_header *)data->buffer; 650 payload = (struct rtp_payload *)(data->buffer + sizeof(*header)); 651 652 memset(data->buffer, 0, sizeof(*header) + sizeof(*payload)); 653 654 payload->frame_count = data->frame_count; 655 header->v = 2; 656 header->pt = 1; 657 header->sequence_number = htons(data->seq_num); 658 header->timestamp = htonl(data->nsamples); 659 header->ssrc = htonl(1); 660 661 data->stream.revents = 0; 662 #ifdef ENABLE_TIMING 663 begin2 = get_microseconds(); 664 #endif 665 ret = poll(&data->stream, 1, POLL_TIMEOUT); 666 #ifdef ENABLE_TIMING 667 end2 = get_microseconds(); 668 print_time("poll", begin2, end2); 669 #endif 670 if (ret == 1 && data->stream.revents == POLLOUT) { 671 long ahead = 0; 672 now = get_microseconds(); 673 674 if (data->next_write) { 675 ahead = data->next_write - now; 676 #ifdef ENABLE_TIMING 677 DBG("duration: %ld, ahead: %ld", duration, ahead); 678 #endif 679 if (ahead > 0) { 680 /* too fast, need to throttle */ 681 usleep(ahead); 682 } 683 } else { 684 data->next_write = now; 685 } 686 if (ahead <= -CATCH_UP_TIMEOUT * 1000) { 687 /* fallen too far behind, don't try to catch up */ 688 VDBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000); 689 data->next_write = 0; 690 } else { 691 data->next_write += duration; 692 } 693 694 #ifdef ENABLE_TIMING 695 begin2 = get_microseconds(); 696 #endif 697 ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL); 698 #ifdef ENABLE_TIMING 699 end2 = get_microseconds(); 700 print_time("send", begin2, end2); 701 #endif 702 if (ret < 0) { 703 /* can happen during normal remote disconnect */ 704 VDBG("send() failed: %d (errno %s)", ret, strerror(errno)); 705 } 706 if (ret == -EPIPE) { 707 bluetooth_close(data); 708 } 709 } else { 710 /* can happen during normal remote disconnect */ 711 VDBG("poll() failed: %d (revents = %d, errno %s)", 712 ret, data->stream.revents, strerror(errno)); 713 data->next_write = 0; 714 } 715 716 /* Reset buffer of data to send */ 717 data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload); 718 data->frame_count = 0; 719 data->samples = 0; 720 data->seq_num++; 721 722 #ifdef ENABLE_TIMING 723 end = get_microseconds(); 724 print_time("avdtp_write", begin, end); 725 #endif 726 return 0; /* always return success */ 727 } 728 729 static int audioservice_send(struct bluetooth_data *data, 730 const bt_audio_msg_header_t *msg) 731 { 732 int err; 733 uint16_t length; 734 735 length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE; 736 737 VDBG("sending %s", bt_audio_strtype(msg->type)); 738 if (send(data->server.fd, msg, length, 739 MSG_NOSIGNAL) > 0) 740 err = 0; 741 else { 742 err = -errno; 743 ERR("Error sending data to audio service: %s(%d)", 744 strerror(errno), errno); 745 if (err == -EPIPE) 746 bluetooth_close(data); 747 } 748 749 return err; 750 } 751 752 static int audioservice_recv(struct bluetooth_data *data, 753 bt_audio_msg_header_t *inmsg) 754 { 755 int err, ret; 756 const char *type, *name; 757 uint16_t length; 758 759 length = inmsg->length ? inmsg->length : BT_SUGGESTED_BUFFER_SIZE; 760 761 ret = recv(data->server.fd, inmsg, length, 0); 762 if (ret < 0) { 763 err = -errno; 764 ERR("Error receiving IPC data from bluetoothd: %s (%d)", 765 strerror(errno), errno); 766 if (err == -EPIPE) 767 bluetooth_close(data); 768 } else if ((size_t) ret < sizeof(bt_audio_msg_header_t)) { 769 ERR("Too short (%d bytes) IPC packet from bluetoothd", ret); 770 err = -EINVAL; 771 } else if (inmsg->type == BT_ERROR) { 772 bt_audio_error_t *error = (bt_audio_error_t *)inmsg; 773 ret = recv(data->server.fd, &error->posix_errno, 774 sizeof(error->posix_errno), 0); 775 if (ret < 0) { 776 err = -errno; 777 ERR("Error receiving error code for BT_ERROR: %s (%d)", 778 strerror(errno), errno); 779 if (err == -EPIPE) 780 bluetooth_close(data); 781 } else { 782 ERR("%s failed : %s(%d)", 783 bt_audio_strname(error->h.name), 784 strerror(error->posix_errno), 785 error->posix_errno); 786 err = -error->posix_errno; 787 } 788 } else { 789 type = bt_audio_strtype(inmsg->type); 790 name = bt_audio_strname(inmsg->name); 791 if (type && name) { 792 DBG("Received %s - %s", type, name); 793 err = 0; 794 } else { 795 err = -EINVAL; 796 ERR("Bogus message type %d - name %d" 797 " received from audio service", 798 inmsg->type, inmsg->name); 799 } 800 801 } 802 return err; 803 } 804 805 static int audioservice_expect(struct bluetooth_data *data, 806 bt_audio_msg_header_t *rsp_hdr, int expected_name) 807 { 808 int err = audioservice_recv(data, rsp_hdr); 809 810 if (err != 0) 811 return err; 812 813 if (rsp_hdr->name != expected_name) { 814 err = -EINVAL; 815 ERR("Bogus message %s received while %s was expected", 816 bt_audio_strname(rsp_hdr->name), 817 bt_audio_strname(expected_name)); 818 } 819 return err; 820 821 } 822 823 static int bluetooth_init(struct bluetooth_data *data) 824 { 825 int sk, err; 826 struct timeval tv = {.tv_sec = RECV_TIMEOUT}; 827 828 DBG("bluetooth_init"); 829 830 sk = bt_audio_service_open(); 831 if (sk < 0) { 832 ERR("bt_audio_service_open failed\n"); 833 return -errno; 834 } 835 836 err = setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); 837 if (err < 0) { 838 ERR("bluetooth_init setsockopt(SO_RCVTIMEO) failed %d", err); 839 return err; 840 } 841 842 data->server.fd = sk; 843 data->server.events = POLLIN; 844 data->state = A2DP_STATE_INITIALIZED; 845 846 return 0; 847 } 848 849 static int bluetooth_parse_capabilities(struct bluetooth_data *data, 850 struct bt_get_capabilities_rsp *rsp) 851 { 852 int bytes_left = rsp->h.length - sizeof(*rsp); 853 codec_capabilities_t *codec = (void *) rsp->data; 854 855 if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP) 856 return -EINVAL; 857 858 while (bytes_left > 0) { 859 if ((codec->type == BT_A2DP_SBC_SINK) && 860 !(codec->lock & BT_WRITE_LOCK)) 861 break; 862 863 if (codec->length == 0) { 864 ERR("bluetooth_parse_capabilities() invalid codec capabilities length"); 865 return -EINVAL; 866 } 867 bytes_left -= codec->length; 868 codec = (codec_capabilities_t *)((char *)codec + codec->length); 869 } 870 871 if (bytes_left <= 0 || 872 codec->length != sizeof(data->sbc_capabilities)) 873 return -EINVAL; 874 875 memcpy(&data->sbc_capabilities, codec, codec->length); 876 return 0; 877 } 878 879 880 static int bluetooth_configure(struct bluetooth_data *data) 881 { 882 char buf[BT_SUGGESTED_BUFFER_SIZE]; 883 struct bt_get_capabilities_req *getcaps_req = (void*) buf; 884 struct bt_get_capabilities_rsp *getcaps_rsp = (void*) buf; 885 int err; 886 887 DBG("bluetooth_configure"); 888 889 data->state = A2DP_STATE_CONFIGURING; 890 memset(getcaps_req, 0, BT_SUGGESTED_BUFFER_SIZE); 891 getcaps_req->h.type = BT_REQUEST; 892 getcaps_req->h.name = BT_GET_CAPABILITIES; 893 894 getcaps_req->flags = 0; 895 getcaps_req->flags |= BT_FLAG_AUTOCONNECT; 896 strncpy(getcaps_req->destination, data->address, 18); 897 getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP; 898 getcaps_req->h.length = sizeof(*getcaps_req); 899 900 err = audioservice_send(data, &getcaps_req->h); 901 if (err < 0) { 902 ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n"); 903 goto error; 904 } 905 906 getcaps_rsp->h.length = 0; 907 err = audioservice_expect(data, &getcaps_rsp->h, BT_GET_CAPABILITIES); 908 if (err < 0) { 909 ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n"); 910 goto error; 911 } 912 913 err = bluetooth_parse_capabilities(data, getcaps_rsp); 914 if (err < 0) { 915 ERR("bluetooth_parse_capabilities failed err: %d", err); 916 goto error; 917 } 918 919 err = bluetooth_a2dp_hw_params(data); 920 if (err < 0) { 921 ERR("bluetooth_a2dp_hw_params failed err: %d", err); 922 goto error; 923 } 924 925 926 set_state(data, A2DP_STATE_CONFIGURED); 927 return 0; 928 929 error: 930 931 if (data->state == A2DP_STATE_CONFIGURING) 932 bluetooth_close(data); 933 return err; 934 } 935 936 static void set_state(struct bluetooth_data *data, a2dp_state_t state) 937 { 938 data->state = state; 939 pthread_cond_signal(&data->client_wait); 940 } 941 942 static void __set_command(struct bluetooth_data *data, a2dp_command_t command) 943 { 944 VDBG("set_command %d\n", command); 945 data->command = command; 946 pthread_cond_signal(&data->thread_wait); 947 return; 948 } 949 950 static void set_command(struct bluetooth_data *data, a2dp_command_t command) 951 { 952 pthread_mutex_lock(&data->mutex); 953 __set_command(data, command); 954 pthread_mutex_unlock(&data->mutex); 955 } 956 957 /* timeout is in milliseconds */ 958 static int wait_for_start(struct bluetooth_data *data, int timeout) 959 { 960 a2dp_state_t state = data->state; 961 struct timeval tv; 962 struct timespec ts; 963 int err = 0; 964 965 #ifdef ENABLE_TIMING 966 uint64_t begin, end; 967 begin = get_microseconds(); 968 #endif 969 970 gettimeofday(&tv, (struct timezone *) NULL); 971 ts.tv_sec = tv.tv_sec + (timeout / 1000); 972 ts.tv_nsec = (tv.tv_usec + (timeout % 1000) * 1000L ) * 1000L; 973 974 pthread_mutex_lock(&data->mutex); 975 while (state != A2DP_STATE_STARTED) { 976 if (state == A2DP_STATE_NONE) 977 __set_command(data, A2DP_CMD_INIT); 978 else if (state == A2DP_STATE_INITIALIZED) 979 __set_command(data, A2DP_CMD_CONFIGURE); 980 else if (state == A2DP_STATE_CONFIGURED) { 981 __set_command(data, A2DP_CMD_START); 982 } 983 again: 984 err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts); 985 if (err) { 986 /* don't timeout if we're done */ 987 if (data->state == A2DP_STATE_STARTED) { 988 err = 0; 989 break; 990 } 991 if (err == ETIMEDOUT) 992 break; 993 goto again; 994 } 995 996 if (state == data->state) 997 goto again; 998 999 state = data->state; 1000 1001 if (state == A2DP_STATE_NONE) { 1002 err = ENODEV; 1003 break; 1004 } 1005 } 1006 pthread_mutex_unlock(&data->mutex); 1007 1008 #ifdef ENABLE_TIMING 1009 end = get_microseconds(); 1010 print_time("wait_for_start", begin, end); 1011 #endif 1012 1013 /* pthread_cond_timedwait returns positive errors */ 1014 return -err; 1015 } 1016 1017 static void a2dp_free(struct bluetooth_data *data) 1018 { 1019 pthread_cond_destroy(&data->client_wait); 1020 pthread_cond_destroy(&data->thread_wait); 1021 pthread_cond_destroy(&data->thread_start); 1022 pthread_mutex_destroy(&data->mutex); 1023 free(data); 1024 return; 1025 } 1026 1027 static void* a2dp_thread(void *d) 1028 { 1029 struct bluetooth_data* data = (struct bluetooth_data*)d; 1030 a2dp_command_t command = A2DP_CMD_NONE; 1031 int err = 0; 1032 1033 DBG("a2dp_thread started"); 1034 prctl(PR_SET_NAME, (int)"a2dp_thread", 0, 0, 0); 1035 1036 pthread_mutex_lock(&data->mutex); 1037 1038 data->started = 1; 1039 pthread_cond_signal(&data->thread_start); 1040 1041 while (1) 1042 { 1043 while (1) { 1044 pthread_cond_wait(&data->thread_wait, &data->mutex); 1045 1046 /* Initialization needed */ 1047 if (data->state == A2DP_STATE_NONE && 1048 data->command != A2DP_CMD_QUIT) { 1049 err = bluetooth_init(data); 1050 } 1051 1052 /* New state command signaled */ 1053 if (command != data->command) { 1054 command = data->command; 1055 break; 1056 } 1057 } 1058 1059 switch (command) { 1060 case A2DP_CMD_CONFIGURE: 1061 if (data->state != A2DP_STATE_INITIALIZED) 1062 break; 1063 err = bluetooth_configure(data); 1064 break; 1065 1066 case A2DP_CMD_START: 1067 if (data->state != A2DP_STATE_CONFIGURED) 1068 break; 1069 err = bluetooth_start(data); 1070 break; 1071 1072 case A2DP_CMD_STOP: 1073 if (data->state != A2DP_STATE_STARTED) 1074 break; 1075 err = bluetooth_stop(data); 1076 break; 1077 1078 case A2DP_CMD_QUIT: 1079 bluetooth_close(data); 1080 sbc_finish(&data->sbc); 1081 a2dp_free(data); 1082 goto done; 1083 1084 case A2DP_CMD_INIT: 1085 /* already called bluetooth_init() */ 1086 default: 1087 break; 1088 } 1089 // reset last command in case of error to allow 1090 // re-execution of the same command 1091 if (err < 0) { 1092 command = A2DP_CMD_NONE; 1093 } 1094 } 1095 1096 done: 1097 pthread_mutex_unlock(&data->mutex); 1098 DBG("a2dp_thread finished"); 1099 return NULL; 1100 } 1101 1102 int a2dp_init(int rate, int channels, a2dpData* dataPtr) 1103 { 1104 struct bluetooth_data* data; 1105 pthread_attr_t attr; 1106 int err; 1107 1108 DBG("a2dp_init rate: %d channels: %d", rate, channels); 1109 *dataPtr = NULL; 1110 data = malloc(sizeof(struct bluetooth_data)); 1111 if (!data) 1112 return -1; 1113 1114 memset(data, 0, sizeof(struct bluetooth_data)); 1115 data->server.fd = -1; 1116 data->stream.fd = -1; 1117 data->state = A2DP_STATE_NONE; 1118 data->command = A2DP_CMD_NONE; 1119 1120 strncpy(data->address, "00:00:00:00:00:00", 18); 1121 data->rate = rate; 1122 data->channels = channels; 1123 1124 sbc_init(&data->sbc, 0); 1125 1126 pthread_mutex_init(&data->mutex, NULL); 1127 pthread_cond_init(&data->thread_start, NULL); 1128 pthread_cond_init(&data->thread_wait, NULL); 1129 pthread_cond_init(&data->client_wait, NULL); 1130 1131 pthread_mutex_lock(&data->mutex); 1132 data->started = 0; 1133 1134 pthread_attr_init(&attr); 1135 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); 1136 1137 err = pthread_create(&data->thread, &attr, a2dp_thread, data); 1138 if (err) { 1139 /* If the thread create fails we must not wait */ 1140 pthread_mutex_unlock(&data->mutex); 1141 err = -err; 1142 goto error; 1143 } 1144 1145 /* Make sure the state machine is ready and waiting */ 1146 while (!data->started) { 1147 pthread_cond_wait(&data->thread_start, &data->mutex); 1148 } 1149 1150 /* Poke the state machine to get it going */ 1151 pthread_cond_signal(&data->thread_wait); 1152 1153 pthread_mutex_unlock(&data->mutex); 1154 pthread_attr_destroy(&attr); 1155 1156 *dataPtr = data; 1157 return 0; 1158 error: 1159 bluetooth_close(data); 1160 sbc_finish(&data->sbc); 1161 pthread_attr_destroy(&attr); 1162 a2dp_free(data); 1163 1164 return err; 1165 } 1166 1167 void a2dp_set_sink(a2dpData d, const char* address) 1168 { 1169 struct bluetooth_data* data = (struct bluetooth_data*)d; 1170 if (strncmp(data->address, address, 18)) { 1171 strncpy(data->address, address, 18); 1172 set_command(data, A2DP_CMD_INIT); 1173 } 1174 } 1175 1176 int a2dp_write(a2dpData d, const void* buffer, int count) 1177 { 1178 struct bluetooth_data* data = (struct bluetooth_data*)d; 1179 uint8_t* src = (uint8_t *)buffer; 1180 int codesize; 1181 int err, ret = 0; 1182 long frames_left = count; 1183 int encoded; 1184 unsigned int written; 1185 const char *buff; 1186 int did_configure = 0; 1187 #ifdef ENABLE_TIMING 1188 uint64_t begin, end; 1189 DBG("********** a2dp_write **********"); 1190 begin = get_microseconds(); 1191 #endif 1192 1193 err = wait_for_start(data, WRITE_TIMEOUT); 1194 if (err < 0) 1195 return err; 1196 1197 codesize = data->codesize; 1198 1199 while (frames_left >= codesize) { 1200 /* Enough data to encode (sbc wants 512 byte blocks) */ 1201 encoded = sbc_encode(&(data->sbc), src, codesize, 1202 data->buffer + data->count, 1203 sizeof(data->buffer) - data->count, 1204 &written); 1205 if (encoded <= 0) { 1206 ERR("Encoding error %d", encoded); 1207 goto done; 1208 } 1209 VDBG("sbc_encode returned %d, codesize: %d, written: %d\n", 1210 encoded, codesize, written); 1211 1212 src += encoded; 1213 data->count += written; 1214 data->frame_count++; 1215 data->samples += encoded; 1216 data->nsamples += encoded; 1217 1218 /* No space left for another frame then send */ 1219 if ((data->count + written >= data->link_mtu) || 1220 (data->count + written >= BUFFER_SIZE)) { 1221 VDBG("sending packet %d, count %d, link_mtu %u", 1222 data->seq_num, data->count, 1223 data->link_mtu); 1224 err = avdtp_write(data); 1225 if (err < 0) 1226 return err; 1227 } 1228 1229 ret += encoded; 1230 frames_left -= encoded; 1231 } 1232 1233 if (frames_left > 0) 1234 ERR("%ld bytes left at end of a2dp_write\n", frames_left); 1235 1236 done: 1237 #ifdef ENABLE_TIMING 1238 end = get_microseconds(); 1239 print_time("a2dp_write total", begin, end); 1240 #endif 1241 return ret; 1242 } 1243 1244 int a2dp_stop(a2dpData d) 1245 { 1246 struct bluetooth_data* data = (struct bluetooth_data*)d; 1247 DBG("a2dp_stop\n"); 1248 if (!data) 1249 return 0; 1250 1251 set_command(data, A2DP_CMD_STOP); 1252 return 0; 1253 } 1254 1255 void a2dp_cleanup(a2dpData d) 1256 { 1257 struct bluetooth_data* data = (struct bluetooth_data*)d; 1258 DBG("a2dp_cleanup\n"); 1259 set_command(data, A2DP_CMD_QUIT); 1260 } 1261
