android/hal-audio.c

   1 /*
   2  * Copyright (C) 2013 Intel Corporation
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  *
  16  */
  17
  18 #include <errno.h>
  19 #include <pthread.h>
  20 #include <poll.h>
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <sys/socket.h>
  25 #include <sys/un.h>
  26 #include <unistd.h>
  27 #include <arpa/inet.h>
  28 #include <fcntl.h>
  29
  30 #include <hardware/audio.h>
  31 #include <hardware/hardware.h>
  32
  33 #include <sbc/sbc.h>
  34
  35 #include "audio-msg.h"
  36 #include "ipc-common.h"
  37 #include "hal-log.h"
  38 #include "hal-msg.h"
  39 #include "../profiles/audio/a2dp-codecs.h"
  40 #include "../src/shared/util.h"
  41
  42 #define FIXED_A2DP_PLAYBACK_LATENCY_MS 25
  43
  44 #define FIXED_BUFFER_SIZE (20 * 512)
  45
  46 #define MAX_FRAMES_IN_PAYLOAD 15
  47
  48 #define MAX_DELAY       100000 /* 100ms */
  49
  50 #define SBC_QUALITY_MIN_BITPOOL 33
  51 #define SBC_QUALITY_STEP        5
  52
  53 static const uint8_t a2dp_src_uuid[] = {
  54                 0x00, 0x00, 0x11, 0x0a, 0x00, 0x00, 0x10, 0x00,
  55                 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb };
  56
  57 static int listen_sk = -1;
  58 static int audio_sk = -1;
  59
  60 static pthread_t ipc_th = 0;
  61 static pthread_mutex_t sk_mutex = PTHREAD_MUTEX_INITIALIZER;
  62
  63 #if __BYTE_ORDER == __LITTLE_ENDIAN
  64
  65 struct rtp_header {
  66         unsigned cc:4;
  67         unsigned x:1;
  68         unsigned p:1;
  69         unsigned v:2;
  70
  71         unsigned pt:7;
  72         unsigned m:1;
  73
  74         uint16_t sequence_number;
  75         uint32_t timestamp;
  76         uint32_t ssrc;
  77         uint32_t csrc[0];
  78 } __attribute__ ((packed));
  79
  80 struct rtp_payload {
  81         unsigned frame_count:4;
  82         unsigned rfa0:1;
  83         unsigned is_last_fragment:1;
  84         unsigned is_first_fragment:1;
  85         unsigned is_fragmented:1;
  86 } __attribute__ ((packed));
  87
  88 #elif __BYTE_ORDER == __BIG_ENDIAN
  89
  90 struct rtp_header {
  91         unsigned v:2;
  92         unsigned p:1;
  93         unsigned x:1;
  94         unsigned cc:4;
  95
  96         unsigned m:1;
  97         unsigned pt:7;
  98
  99         uint16_t sequence_number;
 100         uint32_t timestamp;
 101         uint32_t ssrc;
 102         uint32_t csrc[0];
 103 } __attribute__ ((packed));
 104
 105 struct rtp_payload {
 106         unsigned is_fragmented:1;
 107         unsigned is_first_fragment:1;
 108         unsigned is_last_fragment:1;
 109         unsigned rfa0:1;
 110         unsigned frame_count:4;
 111 } __attribute__ ((packed));
 112
 113 #else
 114 #error "Unknown byte order"
 115 #endif
 116
 117 struct media_packet {
 118         struct rtp_header hdr;
 119         struct rtp_payload payload;
 120         uint8_t data[0];
 121 };
 122
 123 struct audio_input_config {
 124         uint32_t rate;
 125         uint32_t channels;
 126         audio_format_t format;
 127 };
 128
 129 struct sbc_data {
 130         a2dp_sbc_t sbc;
 131
 132         sbc_t enc;
 133
 134         uint16_t payload_len;
 135
 136         size_t in_frame_len;
 137         size_t in_buf_size;
 138
 139         size_t out_frame_len;
 140
 141         unsigned frame_duration;
 142         unsigned frames_per_packet;
 143 };
 144
 145 static void timespec_add(struct timespec *base, uint64_t time_us,
 146                                                         struct timespec *res)
 147 {
 148         res->tv_sec = base->tv_sec + time_us / 1000000;
 149         res->tv_nsec = base->tv_nsec + (time_us % 1000000) * 1000;
 150
 151         if (res->tv_nsec >= 1000000000) {
 152                 res->tv_sec++;
 153                 res->tv_nsec -= 1000000000;
 154         }
 155 }
 156
 157 static void timespec_diff(struct timespec *a, struct timespec *b,
 158                                                         struct timespec *res)
 159 {
 160         res->tv_sec = a->tv_sec - b->tv_sec;
 161         res->tv_nsec = a->tv_nsec - b->tv_nsec;
 162
 163         if (res->tv_nsec < 0) {
 164                 res->tv_sec--;
 165                 res->tv_nsec += 1000000000; /* 1sec */
 166         }
 167 }
 168
 169 static uint64_t timespec_diff_us(struct timespec *a, struct timespec *b)
 170 {
 171         struct timespec res;
 172
 173         timespec_diff(a, b, &res);
 174
 175         return res.tv_sec * 1000000ll + res.tv_nsec / 1000ll;
 176 }
 177
 178 #if defined(ANDROID)
 179 /*
 180  * Bionic does not have clock_nanosleep() prototype in time.h even though
 181  * it provides its implementation.
 182  */
 183 extern int clock_nanosleep(clockid_t clock_id, int flags,
 184                                         const struct timespec *request,
 185                                         struct timespec *remain);
 186 #endif
 187
 188 static int sbc_get_presets(struct audio_preset *preset, size_t *len);
 189 static int sbc_codec_init(struct audio_preset *preset, uint16_t mtu,
 190                                                         void **codec_data);
 191 static int sbc_cleanup(void *codec_data);
 192 static int sbc_get_config(void *codec_data, struct audio_input_config *config);
 193 static size_t sbc_get_buffer_size(void *codec_data);
 194 static size_t sbc_get_mediapacket_duration(void *codec_data);
 195 static ssize_t sbc_encode_mediapacket(void *codec_data, const uint8_t *buffer,
 196                                         size_t len, struct media_packet *mp,
 197                                         size_t mp_data_len, size_t *written);
 198 static bool sbc_update_qos(void *codec_data, uint8_t op);
 199
 200 #define QOS_POLICY_DEFAULT      0x00
 201 #define QOS_POLICY_DECREASE     0x01
 202
 203 struct audio_codec {
 204         uint8_t type;
 205
 206         int (*get_presets) (struct audio_preset *preset, size_t *len);
 207
 208         int (*init) (struct audio_preset *preset, uint16_t mtu,
 209                                 void **codec_data);
 210         int (*cleanup) (void *codec_data);
 211         int (*get_config) (void *codec_data,
 212                                         struct audio_input_config *config);
 213         size_t (*get_buffer_size) (void *codec_data);
 214         size_t (*get_mediapacket_duration) (void *codec_data);
 215         ssize_t (*encode_mediapacket) (void *codec_data, const uint8_t *buffer,
 216                                         size_t len, struct media_packet *mp,
 217                                         size_t mp_data_len, size_t *written);
 218         bool (*update_qos) (void *codec_data, uint8_t op);
 219 };
 220
 221 static const struct audio_codec audio_codecs[] = {
 222         {
 223                 .type = A2DP_CODEC_SBC,
 224
 225                 .get_presets = sbc_get_presets,
 226
 227                 .init = sbc_codec_init,
 228                 .cleanup = sbc_cleanup,
 229                 .get_config = sbc_get_config,
 230                 .get_buffer_size = sbc_get_buffer_size,
 231                 .get_mediapacket_duration = sbc_get_mediapacket_duration,
 232                 .encode_mediapacket = sbc_encode_mediapacket,
 233                 .update_qos = sbc_update_qos,
 234         }
 235 };
 236
 237 #define NUM_CODECS (sizeof(audio_codecs) / sizeof(audio_codecs[0]))
 238
 239 #define MAX_AUDIO_ENDPOINTS NUM_CODECS
 240
 241 struct audio_endpoint {
 242         uint8_t id;
 243         const struct audio_codec *codec;
 244         void *codec_data;
 245         int fd;
 246
 247         struct media_packet *mp;
 248         size_t mp_data_len;
 249
 250         uint16_t seq;
 251         uint32_t samples;
 252         struct timespec start;
 253
 254         bool resync;
 255 };
 256
 257 static struct audio_endpoint audio_endpoints[MAX_AUDIO_ENDPOINTS];
 258
 259 enum a2dp_state_t {
 260         AUDIO_A2DP_STATE_NONE,
 261         AUDIO_A2DP_STATE_STANDBY,
 262         AUDIO_A2DP_STATE_SUSPENDED,
 263         AUDIO_A2DP_STATE_STARTED
 264 };
 265
 266 struct a2dp_stream_out {
 267         struct audio_stream_out stream;
 268
 269         struct audio_endpoint *ep;
 270         enum a2dp_state_t audio_state;
 271         struct audio_input_config cfg;
 272
 273         uint8_t *downmix_buf;
 274 };
 275
 276 struct a2dp_audio_dev {
 277         struct audio_hw_device dev;
 278         struct a2dp_stream_out *out;
 279 };
 280
 281 static const a2dp_sbc_t sbc_presets[] = {
 282         {
 283                 .frequency = SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000,
 284                 .channel_mode = SBC_CHANNEL_MODE_MONO |
 285                                 SBC_CHANNEL_MODE_DUAL_CHANNEL |
 286                                 SBC_CHANNEL_MODE_STEREO |
 287                                 SBC_CHANNEL_MODE_JOINT_STEREO,
 288                 .subbands = SBC_SUBBANDS_4 | SBC_SUBBANDS_8,
 289                 .allocation_method = SBC_ALLOCATION_SNR |
 290                                         SBC_ALLOCATION_LOUDNESS,
 291                 .block_length = SBC_BLOCK_LENGTH_4 | SBC_BLOCK_LENGTH_8 |
 292                                 SBC_BLOCK_LENGTH_12 | SBC_BLOCK_LENGTH_16,
 293                 .min_bitpool = MIN_BITPOOL,
 294                 .max_bitpool = MAX_BITPOOL
 295         },
 296         {
 297                 .frequency = SBC_SAMPLING_FREQ_44100,
 298                 .channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO,
 299                 .subbands = SBC_SUBBANDS_8,
 300                 .allocation_method = SBC_ALLOCATION_LOUDNESS,
 301                 .block_length = SBC_BLOCK_LENGTH_16,
 302                 .min_bitpool = MIN_BITPOOL,
 303                 .max_bitpool = MAX_BITPOOL
 304         },
 305         {
 306                 .frequency = SBC_SAMPLING_FREQ_48000,
 307                 .channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO,
 308                 .subbands = SBC_SUBBANDS_8,
 309                 .allocation_method = SBC_ALLOCATION_LOUDNESS,
 310                 .block_length = SBC_BLOCK_LENGTH_16,
 311                 .min_bitpool = MIN_BITPOOL,
 312                 .max_bitpool = MAX_BITPOOL
 313         },
 314 };
 315
 316 static int sbc_get_presets(struct audio_preset *preset, size_t *len)
 317 {
 318         int i;
 319         int count;
 320         size_t new_len = 0;
 321         uint8_t *ptr = (uint8_t *) preset;
 322         size_t preset_size = sizeof(*preset) + sizeof(a2dp_sbc_t);
 323
 324         count = sizeof(sbc_presets) / sizeof(sbc_presets[0]);
 325
 326         for (i = 0; i < count; i++) {
 327                 preset = (struct audio_preset *) ptr;
 328
 329                 if (new_len + preset_size > *len)
 330                         break;
 331
 332                 preset->len = sizeof(a2dp_sbc_t);
 333                 memcpy(preset->data, &sbc_presets[i], preset->len);
 334
 335                 new_len += preset_size;
 336                 ptr += preset_size;
 337         }
 338
 339         *len = new_len;
 340
 341         return i;
 342 }
 343
 344 static int sbc_freq2int(uint8_t freq)
 345 {
 346         switch (freq) {
 347         case SBC_SAMPLING_FREQ_16000:
 348                 return 16000;
 349         case SBC_SAMPLING_FREQ_32000:
 350                 return 32000;
 351         case SBC_SAMPLING_FREQ_44100:
 352                 return 44100;
 353         case SBC_SAMPLING_FREQ_48000:
 354                 return 48000;
 355         default:
 356                 return 0;
 357         }
 358 }
 359
 360 static const char *sbc_mode2str(uint8_t mode)
 361 {
 362         switch (mode) {
 363         case SBC_CHANNEL_MODE_MONO:
 364                 return "Mono";
 365         case SBC_CHANNEL_MODE_DUAL_CHANNEL:
 366                 return "DualChannel";
 367         case SBC_CHANNEL_MODE_STEREO:
 368                 return "Stereo";
 369         case SBC_CHANNEL_MODE_JOINT_STEREO:
 370                 return "JointStereo";
 371         default:
 372                 return "(unknown)";
 373         }
 374 }
 375
 376 static int sbc_blocks2int(uint8_t blocks)
 377 {
 378         switch (blocks) {
 379         case SBC_BLOCK_LENGTH_4:
 380                 return 4;
 381         case SBC_BLOCK_LENGTH_8:
 382                 return 8;
 383         case SBC_BLOCK_LENGTH_12:
 384                 return 12;
 385         case SBC_BLOCK_LENGTH_16:
 386                 return 16;
 387         default:
 388                 return 0;
 389         }
 390 }
 391
 392 static int sbc_subbands2int(uint8_t subbands)
 393 {
 394         switch (subbands) {
 395         case SBC_SUBBANDS_4:
 396                 return 4;
 397         case SBC_SUBBANDS_8:
 398                 return 8;
 399         default:
 400                 return 0;
 401         }
 402 }
 403
 404 static const char *sbc_allocation2str(uint8_t allocation)
 405 {
 406         switch (allocation) {
 407         case SBC_ALLOCATION_SNR:
 408                 return "SNR";
 409         case SBC_ALLOCATION_LOUDNESS:
 410                 return "Loudness";
 411         default:
 412                 return "(unknown)";
 413         }
 414 }
 415
 416 static void sbc_init_encoder(struct sbc_data *sbc_data)
 417 {
 418         a2dp_sbc_t *in = &sbc_data->sbc;
 419         sbc_t *out = &sbc_data->enc;
 420
 421         sbc_init_a2dp(out, 0L, in, sizeof(*in));
 422
 423         out->endian = SBC_LE;
 424         out->bitpool = in->max_bitpool;
 425
 426         DBG("frequency=%d channel_mode=%s block_length=%d subbands=%d "
 427                         "allocation=%s bitpool=%d-%d",
 428                         sbc_freq2int(in->frequency),
 429                         sbc_mode2str(in->channel_mode),
 430                         sbc_blocks2int(in->block_length),
 431                         sbc_subbands2int(in->subbands),
 432                         sbc_allocation2str(in->allocation_method),
 433                         in->min_bitpool, in->max_bitpool);
 434 }
 435
 436 static void sbc_codec_calculate(struct sbc_data *sbc_data)
 437 {
 438         size_t in_frame_len;
 439         size_t out_frame_len;
 440         size_t num_frames;
 441
 442         in_frame_len = sbc_get_codesize(&sbc_data->enc);
 443         out_frame_len = sbc_get_frame_length(&sbc_data->enc);
 444         num_frames = sbc_data->payload_len / out_frame_len;
 445
 446         sbc_data->in_frame_len = in_frame_len;
 447         sbc_data->in_buf_size = num_frames * in_frame_len;
 448
 449         sbc_data->out_frame_len = out_frame_len;
 450
 451         sbc_data->frame_duration = sbc_get_frame_duration(&sbc_data->enc);
 452         sbc_data->frames_per_packet = num_frames;
 453
 454         DBG("in_frame_len=%zu out_frame_len=%zu frames_per_packet=%zu",
 455                                 in_frame_len, out_frame_len, num_frames);
 456 }
 457
 458 static int sbc_codec_init(struct audio_preset *preset, uint16_t payload_len,
 459                                                         void **codec_data)
 460 {
 461         struct sbc_data *sbc_data;
 462
 463         if (preset->len != sizeof(a2dp_sbc_t)) {
 464                 error("SBC: preset size mismatch");
 465                 return AUDIO_STATUS_FAILED;
 466         }
 467
 468         sbc_data = calloc(sizeof(struct sbc_data), 1);
 469         if (!sbc_data)
 470                 return AUDIO_STATUS_FAILED;
 471
 472         memcpy(&sbc_data->sbc, preset->data, preset->len);
 473
 474         sbc_init_encoder(sbc_data);
 475
 476         sbc_data->payload_len = payload_len;
 477
 478         sbc_codec_calculate(sbc_data);
 479
 480         *codec_data = sbc_data;
 481
 482         return AUDIO_STATUS_SUCCESS;
 483 }
 484
 485 static int sbc_cleanup(void *codec_data)
 486 {
 487         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 488
 489         sbc_finish(&sbc_data->enc);
 490         free(codec_data);
 491
 492         return AUDIO_STATUS_SUCCESS;
 493 }
 494
 495 static int sbc_get_config(void *codec_data, struct audio_input_config *config)
 496 {
 497         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 498
 499         switch (sbc_data->sbc.frequency) {
 500         case SBC_SAMPLING_FREQ_16000:
 501                 config->rate = 16000;
 502                 break;
 503         case SBC_SAMPLING_FREQ_32000:
 504                 config->rate = 32000;
 505                 break;
 506         case SBC_SAMPLING_FREQ_44100:
 507                 config->rate = 44100;
 508                 break;
 509         case SBC_SAMPLING_FREQ_48000:
 510                 config->rate = 48000;
 511                 break;
 512         default:
 513                 return AUDIO_STATUS_FAILED;
 514         }
 515         config->channels = sbc_data->sbc.channel_mode == SBC_CHANNEL_MODE_MONO ?
 516                                 AUDIO_CHANNEL_OUT_MONO :
 517                                 AUDIO_CHANNEL_OUT_STEREO;
 518         config->format = AUDIO_FORMAT_PCM_16_BIT;
 519
 520         return AUDIO_STATUS_SUCCESS;
 521 }
 522
 523 static size_t sbc_get_buffer_size(void *codec_data)
 524 {
 525         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 526
 527         return sbc_data->in_buf_size;
 528 }
 529
 530 static size_t sbc_get_mediapacket_duration(void *codec_data)
 531 {
 532         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 533
 534         return sbc_data->frame_duration * sbc_data->frames_per_packet;
 535 }
 536
 537 static ssize_t sbc_encode_mediapacket(void *codec_data, const uint8_t *buffer,
 538                                         size_t len, struct media_packet *mp,
 539                                         size_t mp_data_len, size_t *written)
 540 {
 541         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 542         size_t consumed = 0;
 543         size_t encoded = 0;
 544         uint8_t frame_count = 0;
 545
 546         while (len - consumed >= sbc_data->in_frame_len &&
 547                         mp_data_len - encoded >= sbc_data->out_frame_len &&
 548                         frame_count < MAX_FRAMES_IN_PAYLOAD) {
 549                 ssize_t read;
 550                 ssize_t written = 0;
 551
 552                 read = sbc_encode(&sbc_data->enc, buffer + consumed,
 553                                 sbc_data->in_frame_len, mp->data + encoded,
 554                                 mp_data_len - encoded, &written);
 555
 556                 if (read < 0) {
 557                         error("SBC: failed to encode block at frame %d (%zd)",
 558                                                         frame_count, read);
 559                         break;
 560                 }
 561
 562                 frame_count++;
 563                 consumed += read;
 564                 encoded += written;
 565         }
 566
 567         *written = encoded;
 568         mp->payload.frame_count = frame_count;
 569
 570         return consumed;
 571 }
 572
 573 static bool sbc_update_qos(void *codec_data, uint8_t op)
 574 {
 575         struct sbc_data *sbc_data = (struct sbc_data *) codec_data;
 576         uint8_t curr_bitpool = sbc_data->enc.bitpool;
 577         uint8_t new_bitpool = curr_bitpool;
 578
 579         switch (op) {
 580         case QOS_POLICY_DEFAULT:
 581                 new_bitpool = sbc_data->sbc.max_bitpool;
 582                 break;
 583
 584         case QOS_POLICY_DECREASE:
 585                 if (curr_bitpool > SBC_QUALITY_MIN_BITPOOL) {
 586                         new_bitpool = curr_bitpool - SBC_QUALITY_STEP;
 587                         if (new_bitpool < SBC_QUALITY_MIN_BITPOOL)
 588                                 new_bitpool = SBC_QUALITY_MIN_BITPOOL;
 589                 }
 590                 break;
 591         }
 592
 593         if (new_bitpool == curr_bitpool)
 594                 return false;
 595
 596         sbc_data->enc.bitpool = new_bitpool;
 597
 598         sbc_codec_calculate(sbc_data);
 599
 600         info("SBC: bitpool changed: %d -> %d", curr_bitpool, new_bitpool);
 601
 602         return true;
 603 }
 604
 605 static int audio_ipc_cmd(uint8_t service_id, uint8_t opcode, uint16_t len,
 606                         void *param, size_t *rsp_len, void *rsp, int *fd)
 607 {
 608         ssize_t ret;
 609         struct msghdr msg;
 610         struct iovec iv[2];
 611         struct ipc_hdr cmd;
 612         char cmsgbuf[CMSG_SPACE(sizeof(int))];
 613         struct ipc_status s;
 614         size_t s_len = sizeof(s);
 615
 616         pthread_mutex_lock(&sk_mutex);
 617
 618         if (audio_sk < 0) {
 619                 error("audio: Invalid cmd socket passed to audio_ipc_cmd");
 620                 goto failed;
 621         }
 622
 623         if (!rsp || !rsp_len) {
 624                 memset(&s, 0, s_len);
 625                 rsp_len = &s_len;
 626                 rsp = &s;
 627         }
 628
 629         memset(&msg, 0, sizeof(msg));
 630         memset(&cmd, 0, sizeof(cmd));
 631
 632         cmd.service_id = service_id;
 633         cmd.opcode = opcode;
 634         cmd.len = len;
 635
 636         iv[0].iov_base = &cmd;
 637         iv[0].iov_len = sizeof(cmd);
 638
 639         iv[1].iov_base = param;
 640         iv[1].iov_len = len;
 641
 642         msg.msg_iov = iv;
 643         msg.msg_iovlen = 2;
 644
 645         ret = sendmsg(audio_sk, &msg, 0);
 646         if (ret < 0) {
 647                 error("audio: Sending command failed:%s", strerror(errno));
 648                 goto failed;
 649         }
 650
 651         /* socket was shutdown */
 652         if (ret == 0) {
 653                 error("audio: Command socket closed");
 654                 goto failed;
 655         }
 656
 657         memset(&msg, 0, sizeof(msg));
 658         memset(&cmd, 0, sizeof(cmd));
 659
 660         iv[0].iov_base = &cmd;
 661         iv[0].iov_len = sizeof(cmd);
 662
 663         iv[1].iov_base = rsp;
 664         iv[1].iov_len = *rsp_len;
 665
 666         msg.msg_iov = iv;
 667         msg.msg_iovlen = 2;
 668
 669         if (fd) {
 670                 memset(cmsgbuf, 0, sizeof(cmsgbuf));
 671                 msg.msg_control = cmsgbuf;
 672                 msg.msg_controllen = sizeof(cmsgbuf);
 673         }
 674
 675         ret = recvmsg(audio_sk, &msg, 0);
 676         if (ret < 0) {
 677                 error("audio: Receiving command response failed:%s",
 678                                                         strerror(errno));
 679                 goto failed;
 680         }
 681
 682         if (ret < (ssize_t) sizeof(cmd)) {
 683                 error("audio: Too small response received(%zd bytes)", ret);
 684                 goto failed;
 685         }
 686
 687         if (cmd.service_id != service_id) {
 688                 error("audio: Invalid service id (%u vs %u)", cmd.service_id,
 689                                                                 service_id);
 690                 goto failed;
 691         }
 692
 693         if (ret != (ssize_t) (sizeof(cmd) + cmd.len)) {
 694                 error("audio: Malformed response received(%zd bytes)", ret);
 695                 goto failed;
 696         }
 697
 698         if (cmd.opcode != opcode && cmd.opcode != AUDIO_OP_STATUS) {
 699                 error("audio: Invalid opcode received (%u vs %u)",
 700                                                 cmd.opcode, opcode);
 701                 goto failed;
 702         }
 703
 704         if (cmd.opcode == AUDIO_OP_STATUS) {
 705                 struct ipc_status *s = rsp;
 706
 707                 if (sizeof(*s) != cmd.len) {
 708                         error("audio: Invalid status length");
 709                         goto failed;
 710                 }
 711
 712                 if (s->code == AUDIO_STATUS_SUCCESS) {
 713                         error("audio: Invalid success status response");
 714                         goto failed;
 715                 }
 716
 717                 pthread_mutex_unlock(&sk_mutex);
 718
 719                 return s->code;
 720         }
 721
 722         pthread_mutex_unlock(&sk_mutex);
 723
 724         /* Receive auxiliary data in msg */
 725         if (fd) {
 726                 struct cmsghdr *cmsg;
 727
 728                 *fd = -1;
 729
 730                 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg;
 731                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 732                         if (cmsg->cmsg_level == SOL_SOCKET
 733                                         && cmsg->cmsg_type == SCM_RIGHTS) {
 734                                 memcpy(fd, CMSG_DATA(cmsg), sizeof(int));
 735                                 break;
 736                         }
 737                 }
 738
 739                 if (*fd < 0)
 740                         goto failed;
 741         }
 742
 743         if (rsp_len)
 744                 *rsp_len = cmd.len;
 745
 746         return AUDIO_STATUS_SUCCESS;
 747
 748 failed:
 749         /* Some serious issue happen on IPC - recover */
 750         shutdown(audio_sk, SHUT_RDWR);
 751         pthread_mutex_unlock(&sk_mutex);
 752
 753         return AUDIO_STATUS_FAILED;
 754 }
 755
 756 static int ipc_open_cmd(const struct audio_codec *codec)
 757 {
 758         uint8_t buf[BLUEZ_AUDIO_MTU];
 759         struct audio_cmd_open *cmd = (struct audio_cmd_open *) buf;
 760         struct audio_rsp_open rsp;
 761         size_t cmd_len = sizeof(buf) - sizeof(*cmd);
 762         size_t rsp_len = sizeof(rsp);
 763         int result;
 764
 765         DBG("");
 766
 767         memcpy(cmd->uuid, a2dp_src_uuid, sizeof(a2dp_src_uuid));
 768
 769         cmd->codec = codec->type;
 770         cmd->presets = codec->get_presets(cmd->preset, &cmd_len);
 771
 772         cmd_len += sizeof(*cmd);
 773
 774         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_OPEN, cmd_len, cmd,
 775                                 &rsp_len, &rsp, NULL);
 776
 777         if (result != AUDIO_STATUS_SUCCESS)
 778                 return 0;
 779
 780         return rsp.id;
 781 }
 782
 783 static int ipc_close_cmd(uint8_t endpoint_id)
 784 {
 785         struct audio_cmd_close cmd;
 786         int result;
 787
 788         DBG("");
 789
 790         cmd.id = endpoint_id;
 791
 792         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_CLOSE,
 793                                 sizeof(cmd), &cmd, NULL, NULL, NULL);
 794
 795         return result;
 796 }
 797
 798 static int ipc_open_stream_cmd(uint8_t endpoint_id, uint16_t *mtu, int *fd,
 799                                                 struct audio_preset **caps)
 800 {
 801         char buf[BLUEZ_AUDIO_MTU];
 802         struct audio_cmd_open_stream cmd;
 803         struct audio_rsp_open_stream *rsp =
 804                                         (struct audio_rsp_open_stream *) &buf;
 805         size_t rsp_len = sizeof(buf);
 806         int result;
 807
 808         DBG("");
 809
 810         if (!caps)
 811                 return AUDIO_STATUS_FAILED;
 812
 813         cmd.id = endpoint_id;
 814
 815         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_OPEN_STREAM,
 816                                 sizeof(cmd), &cmd, &rsp_len, rsp, fd);
 817         if (result == AUDIO_STATUS_SUCCESS) {
 818                 size_t buf_len = sizeof(struct audio_preset) +
 819                                         rsp->preset[0].len;
 820                 *mtu = rsp->mtu;
 821                 *caps = malloc(buf_len);
 822                 memcpy(*caps, &rsp->preset, buf_len);
 823         } else {
 824                 *caps = NULL;
 825         }
 826
 827         return result;
 828 }
 829
 830 static int ipc_close_stream_cmd(uint8_t endpoint_id)
 831 {
 832         struct audio_cmd_close_stream cmd;
 833         int result;
 834
 835         DBG("");
 836
 837         cmd.id = endpoint_id;
 838
 839         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_CLOSE_STREAM,
 840                                         sizeof(cmd), &cmd, NULL, NULL, NULL);
 841
 842         return result;
 843 }
 844
 845 static int ipc_resume_stream_cmd(uint8_t endpoint_id)
 846 {
 847         struct audio_cmd_resume_stream cmd;
 848         int result;
 849
 850         DBG("");
 851
 852         cmd.id = endpoint_id;
 853
 854         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_RESUME_STREAM,
 855                                         sizeof(cmd), &cmd, NULL, NULL, NULL);
 856
 857         return result;
 858 }
 859
 860 static int ipc_suspend_stream_cmd(uint8_t endpoint_id)
 861 {
 862         struct audio_cmd_suspend_stream cmd;
 863         int result;
 864
 865         DBG("");
 866
 867         cmd.id = endpoint_id;
 868
 869         result = audio_ipc_cmd(AUDIO_SERVICE_ID, AUDIO_OP_SUSPEND_STREAM,
 870                                         sizeof(cmd), &cmd, NULL, NULL, NULL);
 871
 872         return result;
 873 }
 874
 875 static int register_endpoints(void)
 876 {
 877         struct audio_endpoint *ep = &audio_endpoints[0];
 878         size_t i;
 879
 880         for (i = 0; i < NUM_CODECS; i++, ep++) {
 881                 const struct audio_codec *codec = &audio_codecs[i];
 882
 883                 ep->id = ipc_open_cmd(codec);
 884
 885                 if (!ep->id)
 886                         return AUDIO_STATUS_FAILED;
 887
 888                 ep->codec = codec;
 889                 ep->codec_data = NULL;
 890                 ep->fd = -1;
 891         }
 892
 893         return AUDIO_STATUS_SUCCESS;
 894 }
 895
 896 static void unregister_endpoints(void)
 897 {
 898         size_t i;
 899
 900         for (i = 0; i < MAX_AUDIO_ENDPOINTS; i++) {
 901                 struct audio_endpoint *ep = &audio_endpoints[i];
 902
 903                 if (ep->id) {
 904                         ipc_close_cmd(ep->id);
 905                         memset(ep, 0, sizeof(*ep));
 906                 }
 907         }
 908 }
 909
 910 static bool open_endpoint(struct audio_endpoint *ep,
 911                                                 struct audio_input_config *cfg)
 912 {
 913         struct audio_preset *preset;
 914         const struct audio_codec *codec;
 915         uint16_t mtu;
 916         uint16_t payload_len;
 917         int fd;
 918
 919         if (ipc_open_stream_cmd(ep->id, &mtu, &fd, &preset) !=
 920                                                         AUDIO_STATUS_SUCCESS)
 921                 return false;
 922
 923         DBG("mtu=%u", mtu);
 924
 925         payload_len = mtu - sizeof(*ep->mp);
 926
 927         ep->fd = fd;
 928
 929         codec = ep->codec;
 930         codec->init(preset, payload_len, &ep->codec_data);
 931         codec->get_config(ep->codec_data, cfg);
 932
 933         ep->mp = calloc(mtu, 1);
 934         if (!ep->mp)
 935                 goto failed;
 936         ep->mp->hdr.v = 2;
 937         ep->mp->hdr.pt = 1;
 938         ep->mp->hdr.ssrc = htonl(1);
 939
 940         ep->mp_data_len = payload_len;
 941
 942         free(preset);
 943
 944         return true;
 945
 946 failed:
 947         close(fd);
 948         free(preset);
 949
 950         return false;
 951 }
 952
 953 static void close_endpoint(struct audio_endpoint *ep)
 954 {
 955         ipc_close_stream_cmd(ep->id);
 956         if (ep->fd >= 0) {
 957                 close(ep->fd);
 958                 ep->fd = -1;
 959         }
 960
 961         free(ep->mp);
 962
 963         ep->codec->cleanup(ep->codec_data);
 964         ep->codec_data = NULL;
 965 }
 966
 967 static bool resume_endpoint(struct audio_endpoint *ep)
 968 {
 969         if (ipc_resume_stream_cmd(ep->id) != AUDIO_STATUS_SUCCESS)
 970                 return false;
 971
 972         ep->samples = 0;
 973         ep->resync = false;
 974
 975         ep->codec->update_qos(ep->codec_data, QOS_POLICY_DEFAULT);
 976
 977         return true;
 978 }
 979
 980 static void downmix_to_mono(struct a2dp_stream_out *out, const uint8_t *buffer,
 981                                                                 size_t bytes)
 982 {
 983         const int16_t *input = (const void *) buffer;
 984         int16_t *output = (void *) out->downmix_buf;
 985         size_t i;
 986
 987         for (i = 0; i < bytes / 2; i++) {
 988                 int16_t l = le16_to_cpu(get_unaligned(&input[i * 2]));
 989                 int16_t r = le16_to_cpu(get_unaligned(&input[i * 2 + 1]));
 990
 991                 put_unaligned(cpu_to_le16((l + r) / 2), &output[i]);
 992         }
 993 }
 994
 995 static bool wait_for_endpoint(struct audio_endpoint *ep, bool *writable)
 996 {
 997         int ret;
 998
 999         while (true) {
1000                 struct pollfd pollfd;
1001
1002                 pollfd.fd = ep->fd;
1003                 pollfd.events = POLLOUT;
1004                 pollfd.revents = 0;
1005
1006                 ret = poll(&pollfd, 1, 500);
1007
1008                 if (ret >= 0) {
1009                         *writable = !!(pollfd.revents & POLLOUT);
1010                         break;
1011                 }
1012
1013                 if (errno != EINTR) {
1014                         ret = errno;
1015                         error("poll failed (%d)", ret);
1016                         return false;
1017                 }
1018         }
1019
1020         return true;
1021 }
1022
1023 static bool write_to_endpoint(struct audio_endpoint *ep, size_t bytes)
1024 {
1025         struct media_packet *mp = (struct media_packet *) ep->mp;
1026         int ret;
1027
1028         while (true) {
1029                 ret = write(ep->fd, mp, sizeof(*mp) + bytes);
1030
1031                 if (ret >= 0)
1032                         break;
1033
1034                 /*
1035                  * this should not happen so let's issue warning, but do not
1036                  * fail, we can try to write next packet
1037                  */
1038                 if (errno == EAGAIN) {
1039                         ret = errno;
1040                         warn("write failed (%d)", ret);
1041                         break;
1042                 }
1043
1044                 if (errno != EINTR) {
1045                         ret = errno;
1046                         error("write failed (%d)", ret);
1047                         return false;
1048                 }
1049         }
1050
1051         return true;
1052 }
1053
1054 static bool write_data(struct a2dp_stream_out *out, const void *buffer,
1055                                                                 size_t bytes)
1056 {
1057         struct audio_endpoint *ep = out->ep;
1058         struct media_packet *mp = (struct media_packet *) ep->mp;
1059         size_t free_space = ep->mp_data_len;
1060         size_t consumed = 0;
1061
1062         while (consumed < bytes) {
1063                 size_t written = 0;
1064                 ssize_t read;
1065                 uint32_t samples;
1066                 int ret;
1067                 struct timespec current;
1068                 uint64_t audio_sent, audio_passed;
1069                 bool do_write = false;
1070
1071                 /*
1072                  * prepare media packet in advance so we don't waste time after
1073                  * wakeup
1074                  */
1075                 mp->hdr.sequence_number = htons(ep->seq++);
1076                 mp->hdr.timestamp = htonl(ep->samples);
1077                 read = ep->codec->encode_mediapacket(ep->codec_data,
1078                                                 buffer + consumed,
1079                                                 bytes - consumed, mp,
1080                                                 free_space, &written);
1081
1082                 /*
1083                  * not much we can do here, let's just ignore remaining
1084                  * data and continue
1085                  */
1086                 if (read <= 0)
1087                         return true;
1088
1089                 /* calculate where are we and where we should be */
1090                 clock_gettime(CLOCK_MONOTONIC, &current);
1091                 if (!ep->samples)
1092                         memcpy(&ep->start, &current, sizeof(ep->start));
1093                 audio_sent = ep->samples * 1000000ll / out->cfg.rate;
1094                 audio_passed = timespec_diff_us(&current, &ep->start);
1095
1096                 /*
1097                  * if we're ahead of stream then wait for next write point,
1098                  * if we're lagging more than 100ms then stop writing and just
1099                  * skip data until we're back in sync
1100                  */
1101                 if (audio_sent > audio_passed) {
1102                         struct timespec anchor;
1103
1104                         ep->resync = false;
1105
1106                         timespec_add(&ep->start, audio_sent, &anchor);
1107
1108                         while (true) {
1109                                 ret = clock_nanosleep(CLOCK_MONOTONIC,
1110                                                         TIMER_ABSTIME, &anchor,
1111                                                         NULL);
1112
1113                                 if (!ret)
1114                                         break;
1115
1116                                 if (ret != EINTR) {
1117                                         error("clock_nanosleep failed (%d)",
1118                                                                         ret);
1119                                         return false;
1120                                 }
1121                         }
1122                 } else if (!ep->resync) {
1123                         uint64_t diff = audio_passed - audio_sent;
1124
1125                         if (diff > MAX_DELAY) {
1126                                 warn("lag is %jums, resyncing", diff / 1000);
1127
1128                                 ep->codec->update_qos(ep->codec_data,
1129                                                         QOS_POLICY_DECREASE);
1130                                 ep->resync = true;
1131                         }
1132                 }
1133
1134                 /* in resync mode we'll just drop mediapackets */
1135                 if (!ep->resync) {
1136                         /* wait some time for socket to be ready for write,
1137                          * but we'll just skip writing data if timeout occurs
1138                          */
1139                         if (!wait_for_endpoint(ep, &do_write))
1140                                 return false;
1141
1142                         if (do_write)
1143                                 if (!write_to_endpoint(ep, written))
1144                                         return false;
1145                 }
1146
1147                 /*
1148                  * AudioFlinger provides 16bit PCM, so sample size is 2 bytes
1149                  * multiplied by number of channels. Number of channels is
1150                  * simply number of bits set in channels mask.
1151                  */
1152                 samples = read / (2 * popcount(out->cfg.channels));
1153                 ep->samples += samples;
1154                 consumed += read;
1155         }
1156
1157         return true;
1158 }
1159
1160 static ssize_t out_write(struct audio_stream_out *stream, const void *buffer,
1161                                                                 size_t bytes)
1162 {
1163         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1164         const void *in_buf = buffer;
1165         size_t in_len = bytes;
1166
1167         /* just return in case we're closing */
1168         if (out->audio_state == AUDIO_A2DP_STATE_NONE)
1169                 return -1;
1170
1171         /* We can auto-start only from standby */
1172         if (out->audio_state == AUDIO_A2DP_STATE_STANDBY) {
1173                 DBG("stream in standby, auto-start");
1174
1175                 if (!resume_endpoint(out->ep))
1176                         return -1;
1177
1178                 out->audio_state = AUDIO_A2DP_STATE_STARTED;
1179         }
1180
1181         if (out->audio_state != AUDIO_A2DP_STATE_STARTED) {
1182                 error("audio: stream not started");
1183                 return -1;
1184         }
1185
1186         if (out->ep->fd < 0) {
1187                 error("audio: no transport socket");
1188                 return -1;
1189         }
1190
1191         /*
1192          * currently Android audioflinger is not able to provide mono stream on
1193          * A2DP output so down mixing needs to be done in hal-audio plugin.
1194          *
1195          * for reference see
1196          * AudioFlinger::PlaybackThread::readOutputParameters()
1197          * frameworks/av/services/audioflinger/Threads.cpp:1631
1198          */
1199         if (out->cfg.channels == AUDIO_CHANNEL_OUT_MONO) {
1200                 if (!out->downmix_buf) {
1201                         error("audio: downmix buffer not initialized");
1202                         return -1;
1203                 }
1204
1205                 downmix_to_mono(out, buffer, bytes);
1206
1207                 in_buf = out->downmix_buf;
1208                 in_len = bytes / 2;
1209         }
1210
1211         if (!write_data(out, in_buf, in_len))
1212                 return -1;
1213
1214         return bytes;
1215 }
1216
1217 static uint32_t out_get_sample_rate(const struct audio_stream *stream)
1218 {
1219         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1220
1221         DBG("");
1222
1223         return out->cfg.rate;
1224 }
1225
1226 static int out_set_sample_rate(struct audio_stream *stream, uint32_t rate)
1227 {
1228         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1229
1230         DBG("");
1231
1232         if (rate != out->cfg.rate) {
1233                 warn("audio: cannot set sample rate to %d", rate);
1234                 return -1;
1235         }
1236
1237         return 0;
1238 }
1239
1240 static size_t out_get_buffer_size(const struct audio_stream *stream)
1241 {
1242         DBG("");
1243
1244         /*
1245          * We should return proper buffer size calculated by codec (so each
1246          * input buffer is encoded into single media packed) but this does not
1247          * work well with AudioFlinger and causes problems. For this reason we
1248          * use magic value here and out_write code takes care of splitting
1249          * input buffer into multiple media packets.
1250          */
1251         return FIXED_BUFFER_SIZE;
1252 }
1253
1254 static uint32_t out_get_channels(const struct audio_stream *stream)
1255 {
1256         DBG("");
1257
1258         /*
1259          * AudioFlinger can only provide stereo stream, so we return it here and
1260          * later we'll downmix this to mono in case codec requires it
1261          */
1262
1263         return AUDIO_CHANNEL_OUT_STEREO;
1264 }
1265
1266 static audio_format_t out_get_format(const struct audio_stream *stream)
1267 {
1268         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1269
1270         DBG("");
1271
1272         return out->cfg.format;
1273 }
1274
1275 static int out_set_format(struct audio_stream *stream, audio_format_t format)
1276 {
1277         DBG("");
1278         return -ENOSYS;
1279 }
1280
1281 static int out_standby(struct audio_stream *stream)
1282 {
1283         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1284
1285         DBG("");
1286
1287         if (out->audio_state == AUDIO_A2DP_STATE_STARTED) {
1288                 if (ipc_suspend_stream_cmd(out->ep->id) != AUDIO_STATUS_SUCCESS)
1289                         return -1;
1290                 out->audio_state = AUDIO_A2DP_STATE_STANDBY;
1291         }
1292
1293         return 0;
1294 }
1295
1296 static int out_dump(const struct audio_stream *stream, int fd)
1297 {
1298         DBG("");
1299         return -ENOSYS;
1300 }
1301
1302 static int out_set_parameters(struct audio_stream *stream, const char *kvpairs)
1303 {
1304         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1305         char *kvpair;
1306         char *str;
1307         char *saveptr;
1308         bool enter_suspend = false;
1309         bool exit_suspend = false;
1310
1311         DBG("%s", kvpairs);
1312
1313         str = strdup(kvpairs);
1314         if (!str)
1315                 return -ENOMEM;
1316
1317         kvpair = strtok_r(str, ";", &saveptr);
1318
1319         for (; kvpair && *kvpair; kvpair = strtok_r(NULL, ";", &saveptr)) {
1320                 char *keyval;
1321
1322                 keyval = strchr(kvpair, '=');
1323                 if (!keyval)
1324                         continue;
1325
1326                 *keyval = '\0';
1327                 keyval++;
1328
1329                 if (!strcmp(kvpair, "closing")) {
1330                         if (!strcmp(keyval, "true"))
1331                                 out->audio_state = AUDIO_A2DP_STATE_NONE;
1332                 } else if (!strcmp(kvpair, "A2dpSuspended")) {
1333                         if (!strcmp(keyval, "true"))
1334                                 enter_suspend = true;
1335                         else
1336                                 exit_suspend = true;
1337                 }
1338         }
1339
1340         free(str);
1341
1342         if (enter_suspend && out->audio_state == AUDIO_A2DP_STATE_STARTED) {
1343                 if (ipc_suspend_stream_cmd(out->ep->id) != AUDIO_STATUS_SUCCESS)
1344                         return -1;
1345                 out->audio_state = AUDIO_A2DP_STATE_SUSPENDED;
1346         }
1347
1348         if (exit_suspend && out->audio_state == AUDIO_A2DP_STATE_SUSPENDED)
1349                 out->audio_state = AUDIO_A2DP_STATE_STANDBY;
1350
1351         return 0;
1352 }
1353
1354 static char *out_get_parameters(const struct audio_stream *stream,
1355                                                         const char *keys)
1356 {
1357         DBG("");
1358         return strdup("");
1359 }
1360
1361 static uint32_t out_get_latency(const struct audio_stream_out *stream)
1362 {
1363         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1364         struct audio_endpoint *ep = out->ep;
1365         size_t pkt_duration;
1366
1367         DBG("");
1368
1369         pkt_duration = ep->codec->get_mediapacket_duration(ep->codec_data);
1370
1371         return FIXED_A2DP_PLAYBACK_LATENCY_MS + pkt_duration / 1000;
1372 }
1373
1374 static int out_set_volume(struct audio_stream_out *stream, float left,
1375                                                                 float right)
1376 {
1377         DBG("");
1378         /* volume controlled in audioflinger mixer (digital) */
1379         return -ENOSYS;
1380 }
1381
1382 static int out_get_render_position(const struct audio_stream_out *stream,
1383                                                         uint32_t *dsp_frames)
1384 {
1385         DBG("");
1386         return -ENOSYS;
1387 }
1388
1389 static int out_add_audio_effect(const struct audio_stream *stream,
1390                                                         effect_handle_t effect)
1391 {
1392         DBG("");
1393         return -ENOSYS;
1394 }
1395
1396 static int out_remove_audio_effect(const struct audio_stream *stream,
1397                                                         effect_handle_t effect)
1398 {
1399         DBG("");
1400         return -ENOSYS;
1401 }
1402
1403 static uint32_t in_get_sample_rate(const struct audio_stream *stream)
1404 {
1405         DBG("");
1406         return -ENOSYS;
1407 }
1408
1409 static int in_set_sample_rate(struct audio_stream *stream, uint32_t rate)
1410 {
1411         DBG("");
1412         return -ENOSYS;
1413 }
1414
1415 static size_t in_get_buffer_size(const struct audio_stream *stream)
1416 {
1417         DBG("");
1418         return -ENOSYS;
1419 }
1420
1421 static uint32_t in_get_channels(const struct audio_stream *stream)
1422 {
1423         DBG("");
1424         return -ENOSYS;
1425 }
1426
1427 static audio_format_t in_get_format(const struct audio_stream *stream)
1428 {
1429         DBG("");
1430         return -ENOSYS;
1431 }
1432
1433 static int in_set_format(struct audio_stream *stream, audio_format_t format)
1434 {
1435         DBG("");
1436         return -ENOSYS;
1437 }
1438
1439 static int in_standby(struct audio_stream *stream)
1440 {
1441         DBG("");
1442         return -ENOSYS;
1443 }
1444
1445 static int in_dump(const struct audio_stream *stream, int fd)
1446 {
1447         DBG("");
1448         return -ENOSYS;
1449 }
1450
1451 static int in_set_parameters(struct audio_stream *stream, const char *kvpairs)
1452 {
1453         DBG("");
1454         return -ENOSYS;
1455 }
1456
1457 static char *in_get_parameters(const struct audio_stream *stream,
1458                                                         const char *keys)
1459 {
1460         DBG("");
1461         return strdup("");
1462 }
1463
1464 static int in_set_gain(struct audio_stream_in *stream, float gain)
1465 {
1466         DBG("");
1467         return -ENOSYS;
1468 }
1469
1470 static ssize_t in_read(struct audio_stream_in *stream, void *buffer,
1471                                                                 size_t bytes)
1472 {
1473         DBG("");
1474         return -ENOSYS;
1475 }
1476
1477 static uint32_t in_get_input_frames_lost(struct audio_stream_in *stream)
1478 {
1479         DBG("");
1480         return -ENOSYS;
1481 }
1482
1483 static int in_add_audio_effect(const struct audio_stream *stream,
1484                                                         effect_handle_t effect)
1485 {
1486         DBG("");
1487         return -ENOSYS;
1488 }
1489
1490 static int in_remove_audio_effect(const struct audio_stream *stream,
1491                                                         effect_handle_t effect)
1492 {
1493         DBG("");
1494         return -ENOSYS;
1495 }
1496
1497 static int audio_open_output_stream(struct audio_hw_device *dev,
1498                                         audio_io_handle_t handle,
1499                                         audio_devices_t devices,
1500                                         audio_output_flags_t flags,
1501                                         struct audio_config *config,
1502                                         struct audio_stream_out **stream_out)
1503
1504 {
1505         struct a2dp_audio_dev *a2dp_dev = (struct a2dp_audio_dev *) dev;
1506         struct a2dp_stream_out *out;
1507
1508         out = calloc(1, sizeof(struct a2dp_stream_out));
1509         if (!out)
1510                 return -ENOMEM;
1511
1512         DBG("");
1513
1514         out->stream.common.get_sample_rate = out_get_sample_rate;
1515         out->stream.common.set_sample_rate = out_set_sample_rate;
1516         out->stream.common.get_buffer_size = out_get_buffer_size;
1517         out->stream.common.get_channels = out_get_channels;
1518         out->stream.common.get_format = out_get_format;
1519         out->stream.common.set_format = out_set_format;
1520         out->stream.common.standby = out_standby;
1521         out->stream.common.dump = out_dump;
1522         out->stream.common.set_parameters = out_set_parameters;
1523         out->stream.common.get_parameters = out_get_parameters;
1524         out->stream.common.add_audio_effect = out_add_audio_effect;
1525         out->stream.common.remove_audio_effect = out_remove_audio_effect;
1526         out->stream.get_latency = out_get_latency;
1527         out->stream.set_volume = out_set_volume;
1528         out->stream.write = out_write;
1529         out->stream.get_render_position = out_get_render_position;
1530
1531         /* TODO: for now we always use endpoint 0 */
1532         out->ep = &audio_endpoints[0];
1533
1534         if (!open_endpoint(out->ep, &out->cfg))
1535                 goto fail;
1536
1537         DBG("rate=%d channels=%d format=%d", out->cfg.rate,
1538                                         out->cfg.channels, out->cfg.format);
1539
1540         if (out->cfg.channels == AUDIO_CHANNEL_OUT_MONO) {
1541                 out->downmix_buf = malloc(FIXED_BUFFER_SIZE / 2);
1542                 if (!out->downmix_buf)
1543                         goto fail;
1544         }
1545
1546         *stream_out = &out->stream;
1547         a2dp_dev->out = out;
1548
1549         out->audio_state = AUDIO_A2DP_STATE_STANDBY;
1550
1551         return 0;
1552
1553 fail:
1554         error("audio: cannot open output stream");
1555         free(out);
1556         *stream_out = NULL;
1557         return -EIO;
1558 }
1559
1560 static void audio_close_output_stream(struct audio_hw_device *dev,
1561                                         struct audio_stream_out *stream)
1562 {
1563         struct a2dp_audio_dev *a2dp_dev = (struct a2dp_audio_dev *) dev;
1564         struct a2dp_stream_out *out = (struct a2dp_stream_out *) stream;
1565
1566         DBG("");
1567
1568         close_endpoint(a2dp_dev->out->ep);
1569
1570         free(out->downmix_buf);
1571
1572         free(stream);
1573         a2dp_dev->out = NULL;
1574 }
1575
1576 static int audio_set_parameters(struct audio_hw_device *dev,
1577                                                         const char *kvpairs)
1578 {
1579         struct a2dp_audio_dev *a2dp_dev = (struct a2dp_audio_dev *) dev;
1580         struct a2dp_stream_out *out = a2dp_dev->out;
1581
1582         DBG("");
1583
1584         if (!out)
1585                 return 0;
1586
1587         return out->stream.common.set_parameters((struct audio_stream *) out,
1588                                                                 kvpairs);
1589 }
1590
1591 static char *audio_get_parameters(const struct audio_hw_device *dev,
1592                                                         const char *keys)
1593 {
1594         DBG("");
1595         return strdup("");
1596 }
1597
1598 static int audio_init_check(const struct audio_hw_device *dev)
1599 {
1600         DBG("");
1601         return 0;
1602 }
1603
1604 static int audio_set_voice_volume(struct audio_hw_device *dev, float volume)
1605 {
1606         DBG("");
1607         return -ENOSYS;
1608 }
1609
1610 static int audio_set_master_volume(struct audio_hw_device *dev, float volume)
1611 {
1612         DBG("");
1613         return -ENOSYS;
1614 }
1615
1616 static int audio_set_mode(struct audio_hw_device *dev, int mode)
1617 {
1618         DBG("");
1619         return -ENOSYS;
1620 }
1621
1622 static int audio_set_mic_mute(struct audio_hw_device *dev, bool state)
1623 {
1624         DBG("");
1625         return -ENOSYS;
1626 }
1627
1628 static int audio_get_mic_mute(const struct audio_hw_device *dev, bool *state)
1629 {
1630         DBG("");
1631         return -ENOSYS;
1632 }
1633
1634 static size_t audio_get_input_buffer_size(const struct audio_hw_device *dev,
1635                                         const struct audio_config *config)
1636 {
1637         DBG("");
1638         return -ENOSYS;
1639 }
1640
1641 static int audio_open_input_stream(struct audio_hw_device *dev,
1642                                         audio_io_handle_t handle,
1643                                         audio_devices_t devices,
1644                                         struct audio_config *config,
1645                                         struct audio_stream_in **stream_in)
1646 {
1647         struct audio_stream_in *in;
1648
1649         DBG("");
1650
1651         in = calloc(1, sizeof(struct audio_stream_in));
1652         if (!in)
1653                 return -ENOMEM;
1654
1655         in->common.get_sample_rate = in_get_sample_rate;
1656         in->common.set_sample_rate = in_set_sample_rate;
1657         in->common.get_buffer_size = in_get_buffer_size;
1658         in->common.get_channels = in_get_channels;
1659         in->common.get_format = in_get_format;
1660         in->common.set_format = in_set_format;
1661         in->common.standby = in_standby;
1662         in->common.dump = in_dump;
1663         in->common.set_parameters = in_set_parameters;
1664         in->common.get_parameters = in_get_parameters;
1665         in->common.add_audio_effect = in_add_audio_effect;
1666         in->common.remove_audio_effect = in_remove_audio_effect;
1667         in->set_gain = in_set_gain;
1668         in->read = in_read;
1669         in->get_input_frames_lost = in_get_input_frames_lost;
1670
1671         *stream_in = in;
1672
1673         return 0;
1674 }
1675
1676 static void audio_close_input_stream(struct audio_hw_device *dev,
1677                                         struct audio_stream_in *stream_in)
1678 {
1679         DBG("");
1680         free(stream_in);
1681 }
1682
1683 static int audio_dump(const audio_hw_device_t *device, int fd)
1684 {
1685         DBG("");
1686         return -ENOSYS;
1687 }
1688
1689 static int audio_close(hw_device_t *device)
1690 {
1691         struct a2dp_audio_dev *a2dp_dev = (struct a2dp_audio_dev *)device;
1692
1693         DBG("");
1694
1695         unregister_endpoints();
1696
1697         shutdown(listen_sk, SHUT_RDWR);
1698         shutdown(audio_sk, SHUT_RDWR);
1699
1700         pthread_join(ipc_th, NULL);
1701
1702         close(listen_sk);
1703         listen_sk = -1;
1704
1705         free(a2dp_dev);
1706         return 0;
1707 }
1708
1709 static void *ipc_handler(void *data)
1710 {
1711         bool done = false;
1712         struct pollfd pfd;
1713         int sk;
1714
1715         DBG("");
1716
1717         while (!done) {
1718                 DBG("Waiting for connection ...");
1719
1720                 sk = accept(listen_sk, NULL, NULL);
1721                 if (sk < 0) {
1722                         int err = errno;
1723
1724                         if (err == EINTR)
1725                                 continue;
1726
1727                         if (err != ECONNABORTED && err != EINVAL)
1728                                 error("audio: Failed to accept socket: %d (%s)",
1729                                                         err, strerror(err));
1730
1731                         break;
1732                 }
1733
1734                 pthread_mutex_lock(&sk_mutex);
1735                 audio_sk = sk;
1736                 pthread_mutex_unlock(&sk_mutex);
1737
1738                 DBG("Audio IPC: Connected");
1739
1740                 if (register_endpoints() != AUDIO_STATUS_SUCCESS) {
1741                         error("audio: Failed to register endpoints");
1742
1743                         unregister_endpoints();
1744
1745                         pthread_mutex_lock(&sk_mutex);
1746                         shutdown(audio_sk, SHUT_RDWR);
1747                         close(audio_sk);
1748                         audio_sk = -1;
1749                         pthread_mutex_unlock(&sk_mutex);
1750
1751                         continue;
1752                 }
1753
1754                 memset(&pfd, 0, sizeof(pfd));
1755                 pfd.fd = audio_sk;
1756                 pfd.events = POLLHUP | POLLERR | POLLNVAL;
1757
1758                 /* Check if socket is still alive. Empty while loop.*/
1759                 while (poll(&pfd, 1, -1) < 0 && errno == EINTR);
1760
1761                 if (pfd.revents & (POLLHUP | POLLERR | POLLNVAL)) {
1762                         info("Audio HAL: Socket closed");
1763
1764                         pthread_mutex_lock(&sk_mutex);
1765                         close(audio_sk);
1766                         audio_sk = -1;
1767                         pthread_mutex_unlock(&sk_mutex);
1768                 }
1769         }
1770
1771         /* audio_sk is closed at this point, just cleanup endpoints states */
1772         memset(audio_endpoints, 0, sizeof(audio_endpoints));
1773
1774         info("Closing Audio IPC thread");
1775         return NULL;
1776 }
1777
1778 static int audio_ipc_init(void)
1779 {
1780         struct sockaddr_un addr;
1781         int err;
1782         int sk;
1783
1784         DBG("");
1785
1786         sk = socket(PF_LOCAL, SOCK_SEQPACKET, 0);
1787         if (sk < 0) {
1788                 err = -errno;
1789                 error("audio: Failed to create socket: %d (%s)", -err,
1790                                                                 strerror(-err));
1791                 return err;
1792         }
1793
1794         memset(&addr, 0, sizeof(addr));
1795         addr.sun_family = AF_UNIX;
1796
1797         memcpy(addr.sun_path, BLUEZ_AUDIO_SK_PATH,
1798                                         sizeof(BLUEZ_AUDIO_SK_PATH));
1799
1800         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
1801                 err = -errno;
1802                 error("audio: Failed to bind socket: %d (%s)", -err,
1803                                                                 strerror(-err));
1804                 goto failed;
1805         }
1806
1807         if (listen(sk, 1) < 0) {
1808                 err = -errno;
1809                 error("audio: Failed to listen on the socket: %d (%s)", -err,
1810                                                                 strerror(-err));
1811                 goto failed;
1812         }
1813
1814         listen_sk = sk;
1815
1816         err = pthread_create(&ipc_th, NULL, ipc_handler, NULL);
1817         if (err) {
1818                 err = -err;
1819                 ipc_th = 0;
1820                 error("audio: Failed to start Audio IPC thread: %d (%s)",
1821                                                         -err, strerror(-err));
1822                 goto failed;
1823         }
1824
1825         return 0;
1826
1827 failed:
1828         close(sk);
1829         return err;
1830 }
1831
1832 static int audio_open(const hw_module_t *module, const char *name,
1833                                                         hw_device_t **device)
1834 {
1835         struct a2dp_audio_dev *a2dp_dev;
1836         int err;
1837
1838         DBG("");
1839
1840         if (strcmp(name, AUDIO_HARDWARE_INTERFACE)) {
1841                 error("audio: interface %s not matching [%s]", name,
1842                                                 AUDIO_HARDWARE_INTERFACE);
1843                 return -EINVAL;
1844         }
1845
1846         err = audio_ipc_init();
1847         if (err < 0)
1848                 return err;
1849
1850         a2dp_dev = calloc(1, sizeof(struct a2dp_audio_dev));
1851         if (!a2dp_dev)
1852                 return -ENOMEM;
1853
1854         a2dp_dev->dev.common.tag = HARDWARE_DEVICE_TAG;
1855         a2dp_dev->dev.common.version = AUDIO_DEVICE_API_VERSION_CURRENT;
1856         a2dp_dev->dev.common.module = (struct hw_module_t *) module;
1857         a2dp_dev->dev.common.close = audio_close;
1858
1859         a2dp_dev->dev.init_check = audio_init_check;
1860         a2dp_dev->dev.set_voice_volume = audio_set_voice_volume;
1861         a2dp_dev->dev.set_master_volume = audio_set_master_volume;
1862         a2dp_dev->dev.set_mode = audio_set_mode;
1863         a2dp_dev->dev.set_mic_mute = audio_set_mic_mute;
1864         a2dp_dev->dev.get_mic_mute = audio_get_mic_mute;
1865         a2dp_dev->dev.set_parameters = audio_set_parameters;
1866         a2dp_dev->dev.get_parameters = audio_get_parameters;
1867         a2dp_dev->dev.get_input_buffer_size = audio_get_input_buffer_size;
1868         a2dp_dev->dev.open_output_stream = audio_open_output_stream;
1869         a2dp_dev->dev.close_output_stream = audio_close_output_stream;
1870         a2dp_dev->dev.open_input_stream = audio_open_input_stream;
1871         a2dp_dev->dev.close_input_stream = audio_close_input_stream;
1872         a2dp_dev->dev.dump = audio_dump;
1873
1874         /*
1875          * Note that &a2dp_dev->dev.common is the same pointer as a2dp_dev.
1876          * This results from the structure of following structs:a2dp_audio_dev,
1877          * audio_hw_device. We will rely on this later in the code.
1878          */
1879         *device = &a2dp_dev->dev.common;
1880
1881         return 0;
1882 }
1883
1884 static struct hw_module_methods_t hal_module_methods = {
1885         .open = audio_open,
1886 };
1887
1888 struct audio_module HAL_MODULE_INFO_SYM = {
1889         .common = {
1890                 .tag = HARDWARE_MODULE_TAG,
1891                 .version_major = 1,
1892                 .version_minor = 0,
1893                 .id = AUDIO_HARDWARE_MODULE_ID,
1894                 .name = "A2DP Bluez HW HAL",
1895                 .author = "Intel Corporation",
1896                 .methods = &hal_module_methods,
1897         },
1898 };