1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2006-2010 Nokia Corporation 6 * Copyright (C) 2004-2010 Marcel Holtmann <marcel (at) holtmann.org> 7 * Copyright (C) 2009 Lennart Poettering 8 * Copyright (C) 2008 Joao Paulo Rechi Vita 9 * 10 * 11 * This library is free software; you can redistribute it and/or 12 * modify it under the terms of the GNU Lesser General Public 13 * License as published by the Free Software Foundation; either 14 * version 2.1 of the License, or (at your option) any later version. 15 * 16 * This library is distributed in the hope that it will be useful, 17 * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 19 * Lesser General Public License for more details. 20 * 21 * You should have received a copy of the GNU Lesser General Public 22 * License along with this library; if not, write to the Free Software 23 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 24 * 25 */ 26 27 #ifdef HAVE_CONFIG_H 28 #include <config.h> 29 #endif 30 31 #include <stdlib.h> 32 #include <stdio.h> 33 #include <errno.h> 34 #include <string.h> 35 #include <assert.h> 36 #include <libgen.h> 37 #include <unistd.h> 38 #include <fcntl.h> 39 #include <signal.h> 40 41 #include <glib.h> 42 43 #include "ipc.h" 44 #include "sbc.h" 45 46 #define DBG(fmt, arg...) \ 47 printf("debug %s: " fmt "\n" , __FUNCTION__ , ## arg) 48 #define ERR(fmt, arg...) \ 49 fprintf(stderr, "ERROR %s: " fmt "\n" , __FUNCTION__ , ## arg) 50 51 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) 52 53 #ifndef MIN 54 # define MIN(x, y) ((x) < (y) ? (x) : (y)) 55 #endif 56 57 #ifndef MAX 58 # define MAX(x, y) ((x) > (y) ? (x) : (y)) 59 #endif 60 61 #ifndef TRUE 62 # define TRUE (1) 63 #endif 64 65 #ifndef FALSE 66 # define FALSE (0) 67 #endif 68 69 #define YES_NO(t) ((t) ? "yes" : "no") 70 71 #define BUFFER_SIZE 2048 72 #define MAX_BITPOOL 64 73 #define MIN_BITPOOL 2 74 75 struct a2dp_info { 76 sbc_capabilities_t sbc_capabilities; 77 sbc_t sbc; /* Codec data */ 78 int sbc_initialized; /* Keep track if the encoder is initialized */ 79 size_t codesize; /* SBC codesize */ 80 81 void* buffer; /* Codec transfer buffer */ 82 size_t buffer_size; /* Size of the buffer */ 83 84 uint16_t seq_num; /* Cumulative packet sequence */ 85 }; 86 87 struct hsp_info { 88 pcm_capabilities_t pcm_capabilities; 89 }; 90 91 struct userdata { 92 int service_fd; 93 int stream_fd; 94 GIOChannel *stream_channel; 95 guint stream_watch; 96 GIOChannel *gin; /* dude, I am thirsty now */ 97 guint gin_watch; 98 int transport; 99 uint32_t rate; 100 int channels; 101 char *address; 102 struct a2dp_info a2dp; 103 struct hsp_info hsp; 104 size_t link_mtu; 105 size_t block_size; 106 gboolean debug_stream_read : 1; 107 gboolean debug_stream_write : 1; 108 }; 109 110 static struct userdata data = { 111 .service_fd = -1, 112 .stream_fd = -1, 113 .transport = BT_CAPABILITIES_TRANSPORT_A2DP, 114 .rate = 48000, 115 .channels = 2, 116 .address = NULL 117 }; 118 119 static int start_stream(struct userdata *u); 120 static int stop_stream(struct userdata *u); 121 static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data); 122 123 static GMainLoop *main_loop; 124 125 static int service_send(struct userdata *u, const bt_audio_msg_header_t *msg) 126 { 127 int err; 128 uint16_t length; 129 130 assert(u); 131 132 length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE; 133 134 DBG("sending %s:%s", bt_audio_strtype(msg->type), 135 bt_audio_strname(msg->name)); 136 137 if (send(u->service_fd, msg, length, 0) > 0) 138 err = 0; 139 else { 140 err = -errno; 141 ERR("Error sending data to audio service: %s(%d)", 142 strerror(errno), errno); 143 } 144 145 return err; 146 } 147 148 static int service_recv(struct userdata *u, bt_audio_msg_header_t *rsp) 149 { 150 int err; 151 const char *type, *name; 152 uint16_t length; 153 154 assert(u); 155 156 length = rsp->length ? : BT_SUGGESTED_BUFFER_SIZE; 157 158 DBG("trying to receive msg from audio service..."); 159 if (recv(u->service_fd, rsp, length, 0) > 0) { 160 type = bt_audio_strtype(rsp->type); 161 name = bt_audio_strname(rsp->name); 162 if (type && name) { 163 DBG("Received %s - %s", type, name); 164 err = 0; 165 } else { 166 err = -EINVAL; 167 ERR("Bogus message type %d - name %d" 168 "received from audio service", 169 rsp->type, rsp->name); 170 } 171 } else { 172 err = -errno; 173 ERR("Error receiving data from audio service: %s(%d)", 174 strerror(errno), errno); 175 } 176 177 return err; 178 } 179 180 static ssize_t service_expect(struct userdata *u, bt_audio_msg_header_t *rsp, 181 uint8_t expected_name) 182 { 183 int r; 184 185 assert(u); 186 assert(u->service_fd >= 0); 187 assert(rsp); 188 189 if ((r = service_recv(u, rsp)) < 0) 190 return r; 191 192 if ((rsp->type != BT_INDICATION && rsp->type != BT_RESPONSE) || 193 (rsp->name != expected_name)) { 194 if (rsp->type == BT_ERROR && rsp->length == sizeof(bt_audio_error_t)) 195 ERR("Received error condition: %s", 196 strerror(((bt_audio_error_t*) rsp)->posix_errno)); 197 else 198 ERR("Bogus message %s received while %s was expected", 199 bt_audio_strname(rsp->name), 200 bt_audio_strname(expected_name)); 201 return -1; 202 } 203 204 return 0; 205 } 206 207 static int init_bt(struct userdata *u) 208 { 209 assert(u); 210 211 if (u->service_fd != -1) 212 return 0; 213 214 DBG("bt_audio_service_open"); 215 216 u->service_fd = bt_audio_service_open(); 217 if (u->service_fd <= 0) { 218 perror(strerror(errno)); 219 return errno; 220 } 221 222 return 0; 223 } 224 225 static int parse_caps(struct userdata *u, const struct bt_get_capabilities_rsp *rsp) 226 { 227 unsigned char *ptr; 228 uint16_t bytes_left; 229 codec_capabilities_t codec; 230 231 assert(u); 232 assert(rsp); 233 234 bytes_left = rsp->h.length - sizeof(*rsp); 235 236 if (bytes_left < sizeof(codec_capabilities_t)) { 237 ERR("Packet too small to store codec information."); 238 return -1; 239 } 240 241 ptr = ((void *) rsp) + sizeof(*rsp); 242 243 memcpy(&codec, ptr, sizeof(codec)); /** ALIGNMENT? **/ 244 245 DBG("Payload size is %lu %lu", 246 (unsigned long) bytes_left, (unsigned long) sizeof(codec)); 247 248 if (u->transport != codec.transport) { 249 ERR("Got capabilities for wrong codec."); 250 return -1; 251 } 252 253 if (u->transport == BT_CAPABILITIES_TRANSPORT_SCO) { 254 255 if (bytes_left <= 0 || 256 codec.length != sizeof(u->hsp.pcm_capabilities)) 257 return -1; 258 259 assert(codec.type == BT_HFP_CODEC_PCM); 260 261 memcpy(&u->hsp.pcm_capabilities, 262 &codec, sizeof(u->hsp.pcm_capabilities)); 263 264 DBG("Has NREC: %s", 265 YES_NO(u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC)); 266 267 } else if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { 268 269 while (bytes_left > 0) { 270 if (codec.type == BT_A2DP_SBC_SINK && 271 !(codec.lock & BT_WRITE_LOCK)) 272 break; 273 274 bytes_left -= codec.length; 275 ptr += codec.length; 276 memcpy(&codec, ptr, sizeof(codec)); 277 } 278 279 DBG("bytes_left = %d, codec.length = %d", 280 bytes_left, codec.length); 281 282 if (bytes_left <= 0 || 283 codec.length != sizeof(u->a2dp.sbc_capabilities)) 284 return -1; 285 286 assert(codec.type == BT_A2DP_SBC_SINK); 287 288 memcpy(&u->a2dp.sbc_capabilities, &codec, 289 sizeof(u->a2dp.sbc_capabilities)); 290 } else { 291 assert(0); 292 } 293 294 return 0; 295 } 296 297 static int get_caps(struct userdata *u) 298 { 299 union { 300 struct bt_get_capabilities_req getcaps_req; 301 struct bt_get_capabilities_rsp getcaps_rsp; 302 bt_audio_error_t error; 303 uint8_t buf[BT_SUGGESTED_BUFFER_SIZE]; 304 } msg; 305 306 assert(u); 307 308 memset(&msg, 0, sizeof(msg)); 309 msg.getcaps_req.h.type = BT_REQUEST; 310 msg.getcaps_req.h.name = BT_GET_CAPABILITIES; 311 msg.getcaps_req.h.length = sizeof(msg.getcaps_req); 312 313 strncpy(msg.getcaps_req.destination, u->address, 314 sizeof(msg.getcaps_req.destination)); 315 msg.getcaps_req.transport = u->transport; 316 msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT; 317 318 if (service_send(u, &msg.getcaps_req.h) < 0) 319 return -1; 320 321 msg.getcaps_rsp.h.length = 0; 322 if (service_expect(u, &msg.getcaps_rsp.h, BT_GET_CAPABILITIES) < 0) 323 return -1; 324 325 return parse_caps(u, &msg.getcaps_rsp); 326 } 327 328 static uint8_t a2dp_default_bitpool(uint8_t freq, uint8_t mode) 329 { 330 switch (freq) { 331 case BT_SBC_SAMPLING_FREQ_16000: 332 case BT_SBC_SAMPLING_FREQ_32000: 333 return 53; 334 335 case BT_SBC_SAMPLING_FREQ_44100: 336 337 switch (mode) { 338 case BT_A2DP_CHANNEL_MODE_MONO: 339 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 340 return 31; 341 342 case BT_A2DP_CHANNEL_MODE_STEREO: 343 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 344 return 53; 345 346 default: 347 DBG("Invalid channel mode %u", mode); 348 return 53; 349 } 350 351 case BT_SBC_SAMPLING_FREQ_48000: 352 353 switch (mode) { 354 case BT_A2DP_CHANNEL_MODE_MONO: 355 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 356 return 29; 357 358 case BT_A2DP_CHANNEL_MODE_STEREO: 359 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 360 return 51; 361 362 default: 363 DBG("Invalid channel mode %u", mode); 364 return 51; 365 } 366 367 default: 368 DBG("Invalid sampling freq %u", freq); 369 return 53; 370 } 371 } 372 373 static int setup_a2dp(struct userdata *u) 374 { 375 sbc_capabilities_t *cap; 376 int i; 377 378 static const struct { 379 uint32_t rate; 380 uint8_t cap; 381 } freq_table[] = { 382 { 16000U, BT_SBC_SAMPLING_FREQ_16000 }, 383 { 32000U, BT_SBC_SAMPLING_FREQ_32000 }, 384 { 44100U, BT_SBC_SAMPLING_FREQ_44100 }, 385 { 48000U, BT_SBC_SAMPLING_FREQ_48000 } 386 }; 387 388 assert(u); 389 assert(u->transport == BT_CAPABILITIES_TRANSPORT_A2DP); 390 391 cap = &u->a2dp.sbc_capabilities; 392 393 /* Find the lowest freq that is at least as high as the requested 394 * sampling rate */ 395 for (i = 0; (unsigned) i < ARRAY_SIZE(freq_table); i++) 396 if (freq_table[i].rate >= u->rate && 397 (cap->frequency & freq_table[i].cap)) { 398 u->rate = freq_table[i].rate; 399 cap->frequency = freq_table[i].cap; 400 break; 401 } 402 403 if ((unsigned) i >= ARRAY_SIZE(freq_table)) { 404 for (; i >= 0; i--) { 405 if (cap->frequency & freq_table[i].cap) { 406 u->rate = freq_table[i].rate; 407 cap->frequency = freq_table[i].cap; 408 break; 409 } 410 } 411 412 if (i < 0) { 413 DBG("Not suitable sample rate"); 414 return -1; 415 } 416 } 417 418 if (u->channels <= 1) { 419 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) { 420 cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO; 421 u->channels = 1; 422 } else 423 u->channels = 2; 424 } 425 426 if (u->channels >= 2) { 427 u->channels = 2; 428 429 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO) 430 cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO; 431 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO) 432 cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO; 433 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL) 434 cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL; 435 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) { 436 cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO; 437 u->channels = 1; 438 } else { 439 DBG("No supported channel modes"); 440 return -1; 441 } 442 } 443 444 if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16) 445 cap->block_length = BT_A2DP_BLOCK_LENGTH_16; 446 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12) 447 cap->block_length = BT_A2DP_BLOCK_LENGTH_12; 448 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8) 449 cap->block_length = BT_A2DP_BLOCK_LENGTH_8; 450 else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4) 451 cap->block_length = BT_A2DP_BLOCK_LENGTH_4; 452 else { 453 DBG("No supported block lengths"); 454 return -1; 455 } 456 457 if (cap->subbands & BT_A2DP_SUBBANDS_8) 458 cap->subbands = BT_A2DP_SUBBANDS_8; 459 else if (cap->subbands & BT_A2DP_SUBBANDS_4) 460 cap->subbands = BT_A2DP_SUBBANDS_4; 461 else { 462 DBG("No supported subbands"); 463 return -1; 464 } 465 466 if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS) 467 cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS; 468 else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR) 469 cap->allocation_method = BT_A2DP_ALLOCATION_SNR; 470 471 cap->min_bitpool = (uint8_t) MAX(MIN_BITPOOL, cap->min_bitpool); 472 cap->max_bitpool = (uint8_t) MIN( 473 a2dp_default_bitpool(cap->frequency, cap->channel_mode), 474 cap->max_bitpool); 475 476 return 0; 477 } 478 479 static void setup_sbc(struct a2dp_info *a2dp) 480 { 481 sbc_capabilities_t *active_capabilities; 482 483 assert(a2dp); 484 485 active_capabilities = &a2dp->sbc_capabilities; 486 487 if (a2dp->sbc_initialized) 488 sbc_reinit(&a2dp->sbc, 0); 489 else 490 sbc_init(&a2dp->sbc, 0); 491 a2dp->sbc_initialized = TRUE; 492 493 switch (active_capabilities->frequency) { 494 case BT_SBC_SAMPLING_FREQ_16000: 495 a2dp->sbc.frequency = SBC_FREQ_16000; 496 break; 497 case BT_SBC_SAMPLING_FREQ_32000: 498 a2dp->sbc.frequency = SBC_FREQ_32000; 499 break; 500 case BT_SBC_SAMPLING_FREQ_44100: 501 a2dp->sbc.frequency = SBC_FREQ_44100; 502 break; 503 case BT_SBC_SAMPLING_FREQ_48000: 504 a2dp->sbc.frequency = SBC_FREQ_48000; 505 break; 506 default: 507 assert(0); 508 } 509 510 switch (active_capabilities->channel_mode) { 511 case BT_A2DP_CHANNEL_MODE_MONO: 512 a2dp->sbc.mode = SBC_MODE_MONO; 513 break; 514 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: 515 a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL; 516 break; 517 case BT_A2DP_CHANNEL_MODE_STEREO: 518 a2dp->sbc.mode = SBC_MODE_STEREO; 519 break; 520 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: 521 a2dp->sbc.mode = SBC_MODE_JOINT_STEREO; 522 break; 523 default: 524 assert(0); 525 } 526 527 switch (active_capabilities->allocation_method) { 528 case BT_A2DP_ALLOCATION_SNR: 529 a2dp->sbc.allocation = SBC_AM_SNR; 530 break; 531 case BT_A2DP_ALLOCATION_LOUDNESS: 532 a2dp->sbc.allocation = SBC_AM_LOUDNESS; 533 break; 534 default: 535 assert(0); 536 } 537 538 switch (active_capabilities->subbands) { 539 case BT_A2DP_SUBBANDS_4: 540 a2dp->sbc.subbands = SBC_SB_4; 541 break; 542 case BT_A2DP_SUBBANDS_8: 543 a2dp->sbc.subbands = SBC_SB_8; 544 break; 545 default: 546 assert(0); 547 } 548 549 switch (active_capabilities->block_length) { 550 case BT_A2DP_BLOCK_LENGTH_4: 551 a2dp->sbc.blocks = SBC_BLK_4; 552 break; 553 case BT_A2DP_BLOCK_LENGTH_8: 554 a2dp->sbc.blocks = SBC_BLK_8; 555 break; 556 case BT_A2DP_BLOCK_LENGTH_12: 557 a2dp->sbc.blocks = SBC_BLK_12; 558 break; 559 case BT_A2DP_BLOCK_LENGTH_16: 560 a2dp->sbc.blocks = SBC_BLK_16; 561 break; 562 default: 563 assert(0); 564 } 565 566 a2dp->sbc.bitpool = active_capabilities->max_bitpool; 567 a2dp->codesize = (uint16_t) sbc_get_codesize(&a2dp->sbc); 568 } 569 570 static int bt_open(struct userdata *u) 571 { 572 union { 573 struct bt_open_req open_req; 574 struct bt_open_rsp open_rsp; 575 bt_audio_error_t error; 576 uint8_t buf[BT_SUGGESTED_BUFFER_SIZE]; 577 } msg; 578 579 memset(&msg, 0, sizeof(msg)); 580 msg.open_req.h.type = BT_REQUEST; 581 msg.open_req.h.name = BT_OPEN; 582 msg.open_req.h.length = sizeof(msg.open_req); 583 584 strncpy(msg.open_req.destination, u->address, 585 sizeof(msg.open_req.destination)); 586 msg.open_req.seid = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ? 587 u->a2dp.sbc_capabilities.capability.seid : 588 BT_A2DP_SEID_RANGE + 1; 589 msg.open_req.lock = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ? 590 BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK; 591 592 if (service_send(u, &msg.open_req.h) < 0) 593 return -1; 594 595 msg.open_rsp.h.length = sizeof(msg.open_rsp); 596 if (service_expect(u, &msg.open_rsp.h, BT_OPEN) < 0) 597 return -1; 598 599 return 0; 600 } 601 602 static int set_conf(struct userdata *u) 603 { 604 union { 605 struct bt_set_configuration_req setconf_req; 606 struct bt_set_configuration_rsp setconf_rsp; 607 bt_audio_error_t error; 608 uint8_t buf[BT_SUGGESTED_BUFFER_SIZE]; 609 } msg; 610 611 if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { 612 if (setup_a2dp(u) < 0) 613 return -1; 614 } 615 616 memset(&msg, 0, sizeof(msg)); 617 msg.setconf_req.h.type = BT_REQUEST; 618 msg.setconf_req.h.name = BT_SET_CONFIGURATION; 619 msg.setconf_req.h.length = sizeof(msg.setconf_req); 620 621 if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { 622 memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities, 623 sizeof(u->a2dp.sbc_capabilities)); 624 msg.setconf_req.h.length += msg.setconf_req.codec.length - 625 sizeof(msg.setconf_req.codec); 626 } else { 627 msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO; 628 msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1; 629 msg.setconf_req.codec.length = sizeof(pcm_capabilities_t); 630 } 631 632 if (service_send(u, &msg.setconf_req.h) < 0) 633 return -1; 634 635 msg.setconf_rsp.h.length = sizeof(msg.setconf_rsp); 636 if (service_expect(u, &msg.setconf_rsp.h, BT_SET_CONFIGURATION) < 0) 637 return -1; 638 639 u->link_mtu = msg.setconf_rsp.link_mtu; 640 641 /* setup SBC encoder now we agree on parameters */ 642 if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { 643 setup_sbc(&u->a2dp); 644 u->block_size = u->a2dp.codesize; 645 DBG("SBC parameters:\n\tallocation=%u\n" 646 "\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n", 647 u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, 648 u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool); 649 } else 650 u->block_size = u->link_mtu; 651 652 return 0; 653 } 654 655 static int setup_bt(struct userdata *u) 656 { 657 assert(u); 658 659 if (get_caps(u) < 0) 660 return -1; 661 662 DBG("Got device caps"); 663 664 if (bt_open(u) < 0) 665 return -1; 666 667 if (set_conf(u) < 0) 668 return -1; 669 670 return 0; 671 } 672 673 static int init_profile(struct userdata *u) 674 { 675 assert(u); 676 677 return setup_bt(u); 678 } 679 680 static void shutdown_bt(struct userdata *u) 681 { 682 assert(u); 683 684 if (u->stream_fd != -1) { 685 stop_stream(u); 686 DBG("close(stream_fd)"); 687 close(u->stream_fd); 688 u->stream_fd = -1; 689 } 690 691 if (u->service_fd != -1) { 692 DBG("bt_audio_service_close"); 693 bt_audio_service_close(u->service_fd); 694 u->service_fd = -1; 695 } 696 } 697 698 static void make_fd_nonblock(int fd) 699 { 700 int v; 701 702 assert(fd >= 0); 703 assert((v = fcntl(fd, F_GETFL)) >= 0); 704 705 if (!(v & O_NONBLOCK)) 706 assert(fcntl(fd, F_SETFL, v|O_NONBLOCK) >= 0); 707 } 708 709 static void make_socket_low_delay(int fd) 710 { 711 /* FIXME: is this widely supported? */ 712 #ifdef SO_PRIORITY 713 int priority; 714 assert(fd >= 0); 715 716 priority = 6; 717 if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, (void*)&priority, 718 sizeof(priority)) < 0) 719 ERR("SO_PRIORITY failed: %s", strerror(errno)); 720 #endif 721 } 722 723 static int read_stream(struct userdata *u) 724 { 725 int ret = 0; 726 ssize_t l; 727 char *buf; 728 729 assert(u); 730 assert(u->stream_fd >= 0); 731 732 buf = alloca(u->link_mtu); 733 734 for (;;) { 735 l = read(u->stream_fd, buf, u->link_mtu); 736 if (u->debug_stream_read) 737 DBG("read from socket: %lli bytes", (long long) l); 738 if (l <= 0) { 739 if (l < 0 && errno == EINTR) 740 continue; 741 else { 742 ERR("Failed to read date from stream_fd: %s", 743 ret < 0 ? strerror(errno) : "EOF"); 744 return -1; 745 } 746 } else { 747 break; 748 } 749 } 750 751 return ret; 752 } 753 754 /* It's what PulseAudio is doing, not sure it's necessary for this 755 * test */ 756 static ssize_t pa_write(int fd, const void *buf, size_t count) 757 { 758 ssize_t r; 759 760 if ((r = send(fd, buf, count, MSG_NOSIGNAL)) >= 0) 761 return r; 762 763 if (errno != ENOTSOCK) 764 return r; 765 766 return write(fd, buf, count); 767 } 768 769 static int write_stream(struct userdata *u) 770 { 771 int ret = 0; 772 ssize_t l; 773 char *buf; 774 775 assert(u); 776 assert(u->stream_fd >= 0); 777 buf = alloca(u->link_mtu); 778 779 for (;;) { 780 l = pa_write(u->stream_fd, buf, u->link_mtu); 781 if (u->debug_stream_write) 782 DBG("written to socket: %lli bytes", (long long) l); 783 assert(l != 0); 784 if (l < 0) { 785 if (errno == EINTR) 786 continue; 787 else { 788 ERR("Failed to write data: %s", strerror(errno)); 789 ret = -1; 790 break; 791 } 792 } else { 793 assert((size_t)l <= u->link_mtu); 794 break; 795 } 796 } 797 798 return ret; 799 } 800 801 static gboolean stream_cb(GIOChannel *gin, GIOCondition condition, gpointer data) 802 { 803 struct userdata *u; 804 805 assert(u = data); 806 807 if (condition & G_IO_IN) { 808 if (read_stream(u) < 0) 809 goto fail; 810 } else if (condition & G_IO_OUT) { 811 if (write_stream(u) < 0) 812 goto fail; 813 } else { 814 DBG("Got %d", condition); 815 g_main_loop_quit(main_loop); 816 return FALSE; 817 } 818 819 return TRUE; 820 821 fail: 822 stop_stream(u); 823 return FALSE; 824 } 825 826 static int start_stream(struct userdata *u) 827 { 828 union { 829 bt_audio_msg_header_t rsp; 830 struct bt_start_stream_req start_req; 831 struct bt_start_stream_rsp start_rsp; 832 struct bt_new_stream_ind streamfd_ind; 833 bt_audio_error_t error; 834 uint8_t buf[BT_SUGGESTED_BUFFER_SIZE]; 835 } msg; 836 837 assert(u); 838 839 if (u->stream_fd >= 0) 840 return 0; 841 if (u->stream_watch != 0) { 842 g_source_remove(u->stream_watch); 843 u->stream_watch = 0; 844 } 845 if (u->stream_channel != 0) { 846 g_io_channel_unref(u->stream_channel); 847 u->stream_channel = NULL; 848 } 849 850 memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE); 851 msg.start_req.h.type = BT_REQUEST; 852 msg.start_req.h.name = BT_START_STREAM; 853 msg.start_req.h.length = sizeof(msg.start_req); 854 855 if (service_send(u, &msg.start_req.h) < 0) 856 return -1; 857 858 msg.rsp.length = sizeof(msg.start_rsp); 859 if (service_expect(u, &msg.rsp, BT_START_STREAM) < 0) 860 return -1; 861 862 msg.rsp.length = sizeof(msg.streamfd_ind); 863 if (service_expect(u, &msg.rsp, BT_NEW_STREAM) < 0) 864 return -1; 865 866 if ((u->stream_fd = bt_audio_service_get_data_fd(u->service_fd)) < 0) { 867 DBG("Failed to get stream fd from audio service."); 868 return -1; 869 } 870 871 make_fd_nonblock(u->stream_fd); 872 make_socket_low_delay(u->stream_fd); 873 874 assert(u->stream_channel = g_io_channel_unix_new(u->stream_fd)); 875 876 u->stream_watch = g_io_add_watch(u->stream_channel, 877 G_IO_IN|G_IO_OUT|G_IO_ERR|G_IO_HUP|G_IO_NVAL, 878 stream_cb, u); 879 880 return 0; 881 } 882 883 static int stop_stream(struct userdata *u) 884 { 885 union { 886 bt_audio_msg_header_t rsp; 887 struct bt_stop_stream_req stop_req; 888 struct bt_stop_stream_rsp stop_rsp; 889 bt_audio_error_t error; 890 uint8_t buf[BT_SUGGESTED_BUFFER_SIZE]; 891 } msg; 892 int r = 0; 893 894 if (u->stream_fd < 0) 895 return 0; 896 897 assert(u); 898 assert(u->stream_channel); 899 900 g_source_remove(u->stream_watch); 901 u->stream_watch = 0; 902 g_io_channel_unref(u->stream_channel); 903 u->stream_channel = NULL; 904 905 memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE); 906 msg.stop_req.h.type = BT_REQUEST; 907 msg.stop_req.h.name = BT_STOP_STREAM; 908 msg.stop_req.h.length = sizeof(msg.stop_req); 909 910 if (service_send(u, &msg.stop_req.h) < 0) { 911 r = -1; 912 goto done; 913 } 914 915 msg.rsp.length = sizeof(msg.stop_rsp); 916 if (service_expect(u, &msg.rsp, BT_STOP_STREAM) < 0) 917 r = -1; 918 919 done: 920 close(u->stream_fd); 921 u->stream_fd = -1; 922 923 return r; 924 } 925 926 static gboolean sleep_cb(gpointer data) 927 { 928 struct userdata *u; 929 930 assert(u = data); 931 932 u->gin_watch = g_io_add_watch(u->gin, 933 G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, data); 934 935 printf(">>> "); 936 fflush(stdout); 937 938 return FALSE; 939 } 940 941 static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data) 942 { 943 char *line, *tmp; 944 gsize term_pos; 945 GError *error = NULL; 946 struct userdata *u; 947 int success; 948 949 assert(u = data); 950 if (!(condition & G_IO_IN)) { 951 DBG("Got %d", condition); 952 g_main_loop_quit(main_loop); 953 return FALSE; 954 } 955 956 if (g_io_channel_read_line(gin, &line, NULL, &term_pos, &error) != 957 G_IO_STATUS_NORMAL) 958 return FALSE; 959 960 line[term_pos] = '\0'; 961 g_strstrip(line); 962 if ((tmp = strchr(line, '#'))) 963 *tmp = '\0'; 964 success = FALSE; 965 966 #define IF_CMD(cmd) \ 967 if (!success && (success = (strncmp(line, #cmd, strlen(#cmd)) == 0))) 968 969 IF_CMD(quit) { 970 g_main_loop_quit(main_loop); 971 return FALSE; 972 } 973 974 IF_CMD(sleep) { 975 unsigned int seconds; 976 if (sscanf(line, "%*s %d", &seconds) != 1) 977 DBG("sleep SECONDS"); 978 else { 979 g_source_remove(u->gin_watch); 980 g_timeout_add_seconds(seconds, sleep_cb, u); 981 return FALSE; 982 } 983 } 984 985 IF_CMD(debug) { 986 char *what = NULL; 987 int enable; 988 989 if (sscanf(line, "%*s %as %d", &what, &enable) != 1) 990 DBG("debug [stream_read|stream_write] [0|1]"); 991 if (strncmp(what, "stream_read", 12) == 0) { 992 u->debug_stream_read = enable; 993 } else if (strncmp(what, "stream_write", 13) == 0) { 994 u->debug_stream_write = enable; 995 } else { 996 DBG("debug [stream_read|stream_write] [0|1]"); 997 } 998 } 999 1000 IF_CMD(init_bt) { 1001 DBG("%d", init_bt(u)); 1002 } 1003 1004 IF_CMD(init_profile) { 1005 DBG("%d", init_profile(u)); 1006 } 1007 1008 IF_CMD(start_stream) { 1009 DBG("%d", start_stream(u)); 1010 } 1011 1012 IF_CMD(stop_stream) { 1013 DBG("%d", stop_stream(u)); 1014 } 1015 1016 IF_CMD(shutdown_bt) { 1017 shutdown_bt(u); 1018 } 1019 1020 IF_CMD(rate) { 1021 if (sscanf(line, "%*s %d", &u->rate) != 1) 1022 DBG("set with rate RATE"); 1023 DBG("rate %d", u->rate); 1024 } 1025 1026 IF_CMD(bdaddr) { 1027 char *address; 1028 1029 if (sscanf(line, "%*s %as", &address) != 1) 1030 DBG("set with bdaddr BDADDR"); 1031 1032 free(u->address); 1033 1034 u->address = address; 1035 DBG("bdaddr %s", u->address); 1036 } 1037 1038 IF_CMD(profile) { 1039 char *profile = NULL; 1040 1041 if (sscanf(line, "%*s %as", &profile) != 1) 1042 DBG("set with profile [hsp|a2dp]"); 1043 if (strncmp(profile, "hsp", 4) == 0) { 1044 u->transport = BT_CAPABILITIES_TRANSPORT_SCO; 1045 } else if (strncmp(profile, "a2dp", 5) == 0) { 1046 u->transport = BT_CAPABILITIES_TRANSPORT_A2DP; 1047 } else { 1048 DBG("set with profile [hsp|a2dp]"); 1049 } 1050 1051 free(profile); 1052 DBG("profile %s", u->transport == BT_CAPABILITIES_TRANSPORT_SCO ? 1053 "hsp" : "a2dp"); 1054 } 1055 1056 if (!success && strlen(line) != 0) { 1057 DBG("%s, unknown command", line); 1058 } 1059 1060 printf(">>> "); 1061 fflush(stdout); 1062 return TRUE; 1063 } 1064 1065 1066 static void show_usage(char* prgname) 1067 { 1068 printf("%s: ipctest [--interactive] BDADDR\n", basename(prgname)); 1069 } 1070 1071 static void sig_term(int sig) 1072 { 1073 g_main_loop_quit(main_loop); 1074 } 1075 1076 int main(int argc, char *argv[]) 1077 { 1078 if (argc < 2) { 1079 show_usage(argv[0]); 1080 exit(EXIT_FAILURE); 1081 } 1082 1083 assert(main_loop = g_main_loop_new(NULL, FALSE)); 1084 1085 if (strncmp("--interactive", argv[1], 14) == 0) { 1086 if (argc < 3) { 1087 show_usage(argv[0]); 1088 exit(EXIT_FAILURE); 1089 } 1090 1091 data.address = strdup(argv[2]); 1092 1093 signal(SIGTERM, sig_term); 1094 signal(SIGINT, sig_term); 1095 1096 assert(data.gin = g_io_channel_unix_new(fileno(stdin))); 1097 1098 data.gin_watch = g_io_add_watch(data.gin, 1099 G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, &data); 1100 1101 printf(">>> "); 1102 fflush(stdout); 1103 1104 g_main_loop_run(main_loop); 1105 1106 } else { 1107 data.address = strdup(argv[1]); 1108 1109 assert(init_bt(&data) == 0); 1110 1111 assert(init_profile(&data) == 0); 1112 1113 assert(start_stream(&data) == 0); 1114 1115 g_main_loop_run(main_loop); 1116 1117 assert(stop_stream(&data) == 0); 1118 1119 shutdown_bt(&data); 1120 } 1121 1122 g_main_loop_unref(main_loop); 1123 1124 printf("\nExiting\n"); 1125 1126 exit(EXIT_SUCCESS); 1127 1128 return 0; 1129 } 1130
