test/ipctest.c

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