M7350/external/bluetooth/bluez/android/hal-sco.c
2024-09-09 08:57:42 +00:00

914 lines
19 KiB
C

/*
* Copyright (C) 2013 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <errno.h>
#include <pthread.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <unistd.h>
#include <hardware/audio.h>
#include <hardware/hardware.h>
#include <audio_utils/resampler.h>
#include "../src/shared/util.h"
#include "sco-msg.h"
#include "ipc-common.h"
#include "hal-log.h"
#define AUDIO_STREAM_DEFAULT_RATE 44100
#define AUDIO_STREAM_SCO_RATE 8000
#define AUDIO_STREAM_DEFAULT_FORMAT AUDIO_FORMAT_PCM_16_BIT
#define OUT_BUFFER_SIZE 2560
#define OUT_STREAM_FRAMES 2560
#define SOCKET_POLL_TIMEOUT_MS 500
static int listen_sk = -1;
static int ipc_sk = -1;
static pthread_t ipc_th = 0;
static pthread_mutex_t sk_mutex = PTHREAD_MUTEX_INITIALIZER;
struct sco_audio_config {
uint32_t rate;
uint32_t channels;
uint32_t frame_num;
uint16_t mtu;
audio_format_t format;
};
struct sco_stream_out {
struct audio_stream_out stream;
struct sco_audio_config cfg;
int fd;
uint8_t *downmix_buf;
struct resampler_itfe *resampler;
int16_t *resample_buf;
uint32_t resample_frame_num;
};
struct sco_dev {
struct audio_hw_device dev;
struct sco_stream_out *out;
};
/*
* return the minimum frame numbers from resampling between BT stack's rate
* and audio flinger's. For output stream, 'output' shall be true, otherwise
* false for input streams at audio flinger side.
*/
static size_t get_resample_frame_num(uint32_t sco_rate, uint32_t rate,
size_t frame_num, bool output)
{
size_t resample_frames_num = frame_num * sco_rate / rate + output;
DBG("resampler: sco_rate %d frame_num %zd rate %d resample frames %zd",
sco_rate, frame_num, rate, resample_frames_num);
return resample_frames_num;
}
/* SCO IPC functions */
static int sco_ipc_cmd(uint8_t service_id, uint8_t opcode, uint16_t len,
void *param, size_t *rsp_len, void *rsp, int *fd)
{
ssize_t ret;
struct msghdr msg;
struct iovec iv[2];
struct ipc_hdr cmd;
char cmsgbuf[CMSG_SPACE(sizeof(int))];
struct ipc_status s;
size_t s_len = sizeof(s);
pthread_mutex_lock(&sk_mutex);
if (ipc_sk < 0) {
error("sco: Invalid cmd socket passed to sco_ipc_cmd");
goto failed;
}
if (!rsp || !rsp_len) {
memset(&s, 0, s_len);
rsp_len = &s_len;
rsp = &s;
}
memset(&msg, 0, sizeof(msg));
memset(&cmd, 0, sizeof(cmd));
cmd.service_id = service_id;
cmd.opcode = opcode;
cmd.len = len;
iv[0].iov_base = &cmd;
iv[0].iov_len = sizeof(cmd);
iv[1].iov_base = param;
iv[1].iov_len = len;
msg.msg_iov = iv;
msg.msg_iovlen = 2;
ret = sendmsg(ipc_sk, &msg, 0);
if (ret < 0) {
error("sco: Sending command failed:%s", strerror(errno));
goto failed;
}
/* socket was shutdown */
if (ret == 0) {
error("sco: Command socket closed");
goto failed;
}
memset(&msg, 0, sizeof(msg));
memset(&cmd, 0, sizeof(cmd));
iv[0].iov_base = &cmd;
iv[0].iov_len = sizeof(cmd);
iv[1].iov_base = rsp;
iv[1].iov_len = *rsp_len;
msg.msg_iov = iv;
msg.msg_iovlen = 2;
if (fd) {
memset(cmsgbuf, 0, sizeof(cmsgbuf));
msg.msg_control = cmsgbuf;
msg.msg_controllen = sizeof(cmsgbuf);
}
ret = recvmsg(ipc_sk, &msg, 0);
if (ret < 0) {
error("sco: Receiving command response failed:%s",
strerror(errno));
goto failed;
}
if (ret < (ssize_t) sizeof(cmd)) {
error("sco: Too small response received(%zd bytes)", ret);
goto failed;
}
if (cmd.service_id != service_id) {
error("sco: Invalid service id (%u vs %u)", cmd.service_id,
service_id);
goto failed;
}
if (ret != (ssize_t) (sizeof(cmd) + cmd.len)) {
error("sco: Malformed response received(%zd bytes)", ret);
goto failed;
}
if (cmd.opcode != opcode && cmd.opcode != SCO_OP_STATUS) {
error("sco: Invalid opcode received (%u vs %u)",
cmd.opcode, opcode);
goto failed;
}
if (cmd.opcode == SCO_OP_STATUS) {
struct ipc_status *s = rsp;
if (sizeof(*s) != cmd.len) {
error("sco: Invalid status length");
goto failed;
}
if (s->code == SCO_STATUS_SUCCESS) {
error("sco: Invalid success status response");
goto failed;
}
pthread_mutex_unlock(&sk_mutex);
return s->code;
}
pthread_mutex_unlock(&sk_mutex);
/* Receive auxiliary data in msg */
if (fd) {
struct cmsghdr *cmsg;
*fd = -1;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SCM_RIGHTS) {
memcpy(fd, CMSG_DATA(cmsg), sizeof(int));
break;
}
}
if (*fd < 0)
goto failed;
}
if (rsp_len)
*rsp_len = cmd.len;
return SCO_STATUS_SUCCESS;
failed:
/* Some serious issue happen on IPC - recover */
shutdown(ipc_sk, SHUT_RDWR);
pthread_mutex_unlock(&sk_mutex);
return SCO_STATUS_FAILED;
}
static int ipc_connect_sco(int *fd, uint16_t *mtu)
{
struct sco_rsp_connect rsp;
size_t rsp_len = sizeof(rsp);
int ret;
DBG("");
ret = sco_ipc_cmd(SCO_SERVICE_ID, SCO_OP_CONNECT, 0, NULL, &rsp_len,
&rsp, fd);
*mtu = rsp.mtu;
return ret;
}
/* Audio stream functions */
static void downmix_to_mono(struct sco_stream_out *out, const uint8_t *buffer,
size_t frame_num)
{
const int16_t *input = (const void *) buffer;
int16_t *output = (void *) out->downmix_buf;
size_t i;
for (i = 0; i < frame_num; i++) {
int16_t l = le16_to_cpu(get_unaligned(&input[i * 2]));
int16_t r = le16_to_cpu(get_unaligned(&input[i * 2 + 1]));
put_unaligned(cpu_to_le16((l + r) >> 1), &output[i]);
}
}
static bool write_data(struct sco_stream_out *out, const uint8_t *buffer,
size_t bytes)
{
struct pollfd pfd;
size_t len, written = 0;
int ret;
uint16_t mtu = /* out->cfg.mtu */ 48;
uint8_t read_buf[mtu];
bool do_write = false;
pfd.fd = out->fd;
pfd.events = POLLOUT | POLLIN | POLLHUP | POLLNVAL;
while (bytes > written) {
/* poll for sending */
if (poll(&pfd, 1, SOCKET_POLL_TIMEOUT_MS) == 0) {
DBG("timeout fd %d", out->fd);
return false;
}
if (pfd.revents & (POLLHUP | POLLNVAL)) {
error("error fd %d, events 0x%x", out->fd, pfd.revents);
return false;
}
/* FIXME synchronize by time instead of read() */
if (pfd.revents & POLLIN) {
ret = read(out->fd, read_buf, mtu);
if (ret < 0) {
error("Error reading fd %d (%s)", out->fd,
strerror(errno));
return false;
}
do_write = true;
}
if (!do_write)
continue;
len = bytes - written > mtu ? mtu : bytes - written;
ret = write(out->fd, buffer + written, len);
if (ret > 0) {
written += ret;
do_write = false;
continue;
}
if (errno == EAGAIN) {
ret = errno;
warn("write failed (%d)", ret);
continue;
}
if (errno != EINTR) {
ret = errno;
error("write failed (%d) fd %d bytes %zd", ret, out->fd,
bytes);
return false;
}
}
DBG("written %zd bytes", bytes);
return true;
}
static ssize_t out_write(struct audio_stream_out *stream, const void *buffer,
size_t bytes)
{
struct sco_stream_out *out = (struct sco_stream_out *) stream;
size_t frame_num = bytes / audio_stream_frame_size(&out->stream.common);
size_t output_frame_num = frame_num;
void *send_buf = out->downmix_buf;
size_t total;
DBG("write to fd %d bytes %zu", out->fd, bytes);
if (!out->downmix_buf) {
error("sco: downmix buffer not initialized");
return -1;
}
downmix_to_mono(out, buffer, frame_num);
if (out->resampler) {
int ret;
/* limit resampler's output within what resample buf can hold */
output_frame_num = out->resample_frame_num;
ret = out->resampler->resample_from_input(out->resampler,
send_buf,
&frame_num,
out->resample_buf,
&output_frame_num);
if (ret) {
error("Failed to resample frames: %zd input %zd (%s)",
frame_num, output_frame_num, strerror(ret));
return -1;
}
send_buf = out->resample_buf;
DBG("Resampled: frame_num %zd, output_frame_num %zd",
frame_num, output_frame_num);
}
total = output_frame_num * sizeof(int16_t) * 1;
DBG("total %zd", total);
if (!write_data(out, send_buf, total))
return -1;
return bytes;
}
static uint32_t out_get_sample_rate(const struct audio_stream *stream)
{
struct sco_stream_out *out = (struct sco_stream_out *) stream;
DBG("rate %u", out->cfg.rate);
return out->cfg.rate;
}
static int out_set_sample_rate(struct audio_stream *stream, uint32_t rate)
{
DBG("rate %u", rate);
return 0;
}
static size_t out_get_buffer_size(const struct audio_stream *stream)
{
struct sco_stream_out *out = (struct sco_stream_out *) stream;
size_t size = audio_stream_frame_size(&out->stream.common) *
out->cfg.frame_num;
DBG("buf size %zd", size);
return size;
}
static uint32_t out_get_channels(const struct audio_stream *stream)
{
struct sco_stream_out *out = (struct sco_stream_out *) stream;
DBG("channels num: %u", popcount(out->cfg.channels));
return out->cfg.channels;
}
static audio_format_t out_get_format(const struct audio_stream *stream)
{
struct sco_stream_out *out = (struct sco_stream_out *) stream;
DBG("format: %u", out->cfg.format);
return out->cfg.format;
}
static int out_set_format(struct audio_stream *stream, audio_format_t format)
{
DBG("");
return -ENOSYS;
}
static int out_standby(struct audio_stream *stream)
{
DBG("");
return 0;
}
static int out_dump(const struct audio_stream *stream, int fd)
{
DBG("");
return -ENOSYS;
}
static int out_set_parameters(struct audio_stream *stream, const char *kvpairs)
{
DBG("%s", kvpairs);
return 0;
}
static char *out_get_parameters(const struct audio_stream *stream,
const char *keys)
{
DBG("");
return strdup("");
}
static uint32_t out_get_latency(const struct audio_stream_out *stream)
{
DBG("");
return 0;
}
static int out_set_volume(struct audio_stream_out *stream, float left,
float right)
{
DBG("");
return -ENOSYS;
}
static int out_get_render_position(const struct audio_stream_out *stream,
uint32_t *dsp_frames)
{
DBG("");
return -ENOSYS;
}
static int out_add_audio_effect(const struct audio_stream *stream,
effect_handle_t effect)
{
DBG("");
return -ENOSYS;
}
static int out_remove_audio_effect(const struct audio_stream *stream,
effect_handle_t effect)
{
DBG("");
return -ENOSYS;
}
static int sco_open_output_stream(struct audio_hw_device *dev,
audio_io_handle_t handle,
audio_devices_t devices,
audio_output_flags_t flags,
struct audio_config *config,
struct audio_stream_out **stream_out)
{
struct sco_dev *adev = (struct sco_dev *) dev;
struct sco_stream_out *out;
int fd = -1;
int chan_num, ret;
size_t resample_size;
uint16_t mtu;
DBG("");
if (ipc_connect_sco(&fd, &mtu) != SCO_STATUS_SUCCESS) {
error("sco: cannot get fd");
return -EIO;
}
DBG("got sco fd %d mtu %u", fd, mtu);
out = calloc(1, sizeof(struct sco_stream_out));
if (!out)
return -ENOMEM;
out->stream.common.get_sample_rate = out_get_sample_rate;
out->stream.common.set_sample_rate = out_set_sample_rate;
out->stream.common.get_buffer_size = out_get_buffer_size;
out->stream.common.get_channels = out_get_channels;
out->stream.common.get_format = out_get_format;
out->stream.common.set_format = out_set_format;
out->stream.common.standby = out_standby;
out->stream.common.dump = out_dump;
out->stream.common.set_parameters = out_set_parameters;
out->stream.common.get_parameters = out_get_parameters;
out->stream.common.add_audio_effect = out_add_audio_effect;
out->stream.common.remove_audio_effect = out_remove_audio_effect;
out->stream.get_latency = out_get_latency;
out->stream.set_volume = out_set_volume;
out->stream.write = out_write;
out->stream.get_render_position = out_get_render_position;
/* Configuration for Android */
out->cfg.format = AUDIO_STREAM_DEFAULT_FORMAT;
out->cfg.channels = AUDIO_CHANNEL_OUT_STEREO;
out->cfg.rate = AUDIO_STREAM_DEFAULT_RATE;
out->cfg.frame_num = OUT_STREAM_FRAMES;
out->cfg.mtu = mtu;
out->downmix_buf = malloc(out_get_buffer_size(&out->stream.common));
if (!out->downmix_buf) {
free(out);
return -ENOMEM;
}
DBG("size %zd", out_get_buffer_size(&out->stream.common));
/* Channel numbers for resampler */
chan_num = 1;
ret = create_resampler(out->cfg.rate, AUDIO_STREAM_SCO_RATE, chan_num,
RESAMPLER_QUALITY_DEFAULT, NULL,
&out->resampler);
if (ret) {
error("Failed to create resampler (%s)", strerror(ret));
goto failed;
}
DBG("Created resampler: input rate [%d] output rate [%d] channels [%d]",
out->cfg.rate, AUDIO_STREAM_SCO_RATE, chan_num);
out->resample_frame_num = get_resample_frame_num(AUDIO_STREAM_SCO_RATE,
out->cfg.rate,
out->cfg.frame_num, 1);
if (!out->resample_frame_num) {
error("frame num is too small to resample, discard it");
goto failed;
}
resample_size = sizeof(int16_t) * chan_num * out->resample_frame_num;
out->resample_buf = malloc(resample_size);
if (!out->resample_buf) {
error("failed to allocate resample buffer for %u frames",
out->resample_frame_num);
goto failed;
}
DBG("resampler: frame num %u buf size %zd bytes",
out->resample_frame_num, resample_size);
*stream_out = &out->stream;
adev->out = out;
out->fd = fd;
return 0;
failed:
free(out->downmix_buf);
free(out);
stream_out = NULL;
adev->out = NULL;
return ret;
}
static void sco_close_output_stream(struct audio_hw_device *dev,
struct audio_stream_out *stream_out)
{
struct sco_dev *sco_dev = (struct sco_dev *) dev;
struct sco_stream_out *out = (struct sco_stream_out *) stream_out;
DBG("dev %p stream %p fd %d", dev, stream_out, sco_dev->out->fd);
if (sco_dev->out && sco_dev->out->fd) {
close(sco_dev->out->fd);
sco_dev->out->fd = -1;
}
free(out->downmix_buf);
free(out);
sco_dev->out = NULL;
}
static int sco_set_parameters(struct audio_hw_device *dev,
const char *kvpairs)
{
DBG("%s", kvpairs);
return 0;
}
static char *sco_get_parameters(const struct audio_hw_device *dev,
const char *keys)
{
DBG("");
return strdup("");
}
static int sco_init_check(const struct audio_hw_device *dev)
{
DBG("");
return 0;
}
static int sco_set_voice_volume(struct audio_hw_device *dev, float volume)
{
DBG("%f", volume);
return 0;
}
static int sco_set_master_volume(struct audio_hw_device *dev, float volume)
{
DBG("%f", volume);
return 0;
}
static int sco_set_mode(struct audio_hw_device *dev, int mode)
{
DBG("");
return -ENOSYS;
}
static int sco_set_mic_mute(struct audio_hw_device *dev, bool state)
{
DBG("");
return -ENOSYS;
}
static int sco_get_mic_mute(const struct audio_hw_device *dev, bool *state)
{
DBG("");
return -ENOSYS;
}
static size_t sco_get_input_buffer_size(const struct audio_hw_device *dev,
const struct audio_config *config)
{
DBG("");
return -ENOSYS;
}
static int sco_open_input_stream(struct audio_hw_device *dev,
audio_io_handle_t handle,
audio_devices_t devices,
struct audio_config *config,
struct audio_stream_in **stream_in)
{
DBG("");
return 0;
}
static void sco_close_input_stream(struct audio_hw_device *dev,
struct audio_stream_in *stream_in)
{
DBG("");
free(stream_in);
}
static int sco_dump(const audio_hw_device_t *device, int fd)
{
DBG("");
return 0;
}
static int sco_close(hw_device_t *device)
{
DBG("");
free(device);
return 0;
}
static void *ipc_handler(void *data)
{
bool done = false;
struct pollfd pfd;
int sk;
DBG("");
while (!done) {
DBG("Waiting for connection ...");
sk = accept(listen_sk, NULL, NULL);
if (sk < 0) {
int err = errno;
if (err == EINTR)
continue;
if (err != ECONNABORTED && err != EINVAL)
error("sco: Failed to accept socket: %d (%s)",
err, strerror(err));
break;
}
pthread_mutex_lock(&sk_mutex);
ipc_sk = sk;
pthread_mutex_unlock(&sk_mutex);
DBG("SCO IPC: Connected");
memset(&pfd, 0, sizeof(pfd));
pfd.fd = ipc_sk;
pfd.events = POLLHUP | POLLERR | POLLNVAL;
/* Check if socket is still alive. Empty while loop.*/
while (poll(&pfd, 1, -1) < 0 && errno == EINTR);
if (pfd.revents & (POLLHUP | POLLERR | POLLNVAL)) {
info("SCO HAL: Socket closed");
pthread_mutex_lock(&sk_mutex);
close(ipc_sk);
ipc_sk = -1;
pthread_mutex_unlock(&sk_mutex);
}
}
info("Closing SCO IPC thread");
return NULL;
}
static int sco_ipc_init(void)
{
struct sockaddr_un addr;
int err;
int sk;
DBG("");
sk = socket(PF_LOCAL, SOCK_SEQPACKET, 0);
if (sk < 0) {
err = -errno;
error("sco: Failed to create socket: %d (%s)", -err,
strerror(-err));
return err;
}
memset(&addr, 0, sizeof(addr));
addr.sun_family = AF_UNIX;
memcpy(addr.sun_path, BLUEZ_SCO_SK_PATH, sizeof(BLUEZ_SCO_SK_PATH));
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
err = -errno;
error("sco: Failed to bind socket: %d (%s)", -err,
strerror(-err));
goto failed;
}
if (listen(sk, 1) < 0) {
err = -errno;
error("sco: Failed to listen on the socket: %d (%s)", -err,
strerror(-err));
goto failed;
}
listen_sk = sk;
err = pthread_create(&ipc_th, NULL, ipc_handler, NULL);
if (err) {
err = -err;
ipc_th = 0;
error("sco: Failed to start IPC thread: %d (%s)",
-err, strerror(-err));
goto failed;
}
return 0;
failed:
close(sk);
return err;
}
static int sco_open(const hw_module_t *module, const char *name,
hw_device_t **device)
{
struct sco_dev *dev;
int err;
DBG("");
if (strcmp(name, AUDIO_HARDWARE_INTERFACE)) {
error("SCO: interface %s not matching [%s]", name,
AUDIO_HARDWARE_INTERFACE);
return -EINVAL;
}
err = sco_ipc_init();
if (err < 0)
return err;
dev = calloc(1, sizeof(struct sco_dev));
if (!dev)
return -ENOMEM;
dev->dev.common.tag = HARDWARE_DEVICE_TAG;
dev->dev.common.version = AUDIO_DEVICE_API_VERSION_CURRENT;
dev->dev.common.module = (struct hw_module_t *) module;
dev->dev.common.close = sco_close;
dev->dev.init_check = sco_init_check;
dev->dev.set_voice_volume = sco_set_voice_volume;
dev->dev.set_master_volume = sco_set_master_volume;
dev->dev.set_mode = sco_set_mode;
dev->dev.set_mic_mute = sco_set_mic_mute;
dev->dev.get_mic_mute = sco_get_mic_mute;
dev->dev.set_parameters = sco_set_parameters;
dev->dev.get_parameters = sco_get_parameters;
dev->dev.get_input_buffer_size = sco_get_input_buffer_size;
dev->dev.open_output_stream = sco_open_output_stream;
dev->dev.close_output_stream = sco_close_output_stream;
dev->dev.open_input_stream = sco_open_input_stream;
dev->dev.close_input_stream = sco_close_input_stream;
dev->dev.dump = sco_dump;
*device = &dev->dev.common;
return 0;
}
static struct hw_module_methods_t hal_module_methods = {
.open = sco_open,
};
struct audio_module HAL_MODULE_INFO_SYM = {
.common = {
.tag = HARDWARE_MODULE_TAG,
.version_major = 1,
.version_minor = 0,
.id = AUDIO_HARDWARE_MODULE_ID,
.name = "SCO Audio HW HAL",
.author = "Intel Corporation",
.methods = &hal_module_methods,
},
};