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