From 78a3c72f60646ab9a44eab94b3afa4652ddeb8db Mon Sep 17 00:00:00 2001 From: João Paulo Rechi Vita Date: Fri, 29 Aug 2008 20:22:14 -0300 Subject: Move bluetooth discover and device modules to src/modules/bluetooth --- src/modules/bluetooth/ipc.c | 119 ++ src/modules/bluetooth/ipc.h | 308 +++++ src/modules/bluetooth/module-bluetooth-device.c | 949 ++++++++++++++ src/modules/bluetooth/module-bluetooth-discover.c | 532 ++++++++ src/modules/bluetooth/rtp.h | 76 ++ src/modules/bluetooth/sbc.c | 1411 +++++++++++++++++++++ src/modules/bluetooth/sbc.h | 97 ++ src/modules/bluetooth/sbc_math.h | 72 ++ src/modules/bluetooth/sbc_tables.h | 167 +++ 9 files changed, 3731 insertions(+) create mode 100644 src/modules/bluetooth/ipc.c create mode 100644 src/modules/bluetooth/ipc.h create mode 100644 src/modules/bluetooth/module-bluetooth-device.c create mode 100644 src/modules/bluetooth/module-bluetooth-discover.c create mode 100644 src/modules/bluetooth/rtp.h create mode 100644 src/modules/bluetooth/sbc.c create mode 100644 src/modules/bluetooth/sbc.h create mode 100644 src/modules/bluetooth/sbc_math.h create mode 100644 src/modules/bluetooth/sbc_tables.h (limited to 'src/modules/bluetooth') diff --git a/src/modules/bluetooth/ipc.c b/src/modules/bluetooth/ipc.c new file mode 100644 index 00000000..e7b712da --- /dev/null +++ b/src/modules/bluetooth/ipc.c @@ -0,0 +1,119 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2004-2008 Marcel Holtmann + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include "ipc.h" + +/* This table contains the string representation for messages */ +static const char *strmsg[] = { + "BT_GETCAPABILITIES_REQ", + "BT_GETCAPABILITIES_RSP", + "BT_SETCONFIGURATION_REQ", + "BT_SETCONFIGURATION_RSP", + "BT_STREAMSTART_REQ", + "BT_STREAMSTART_RSP", + "BT_STREAMSTOP_REQ", + "BT_STREAMSTOP_RSP", + "BT_STREAMSUSPEND_IND", + "BT_STREAMRESUME_IND", + "BT_CONTROL_REQ", + "BT_CONTROL_RSP", + "BT_CONTROL_IND", + "BT_STREAMFD_IND", +}; + +int bt_audio_service_open() +{ + int sk; + int err; + struct sockaddr_un addr = { + AF_UNIX, BT_IPC_SOCKET_NAME + }; + + sk = socket(PF_LOCAL, SOCK_STREAM, 0); + if (sk < 0) { + err = errno; + fprintf(stderr, "%s: Cannot open socket: %s (%d)\n", + __FUNCTION__, strerror(err), err); + errno = err; + return -1; + } + + if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + err = errno; + fprintf(stderr, "%s: connect() failed: %s (%d)\n", + __FUNCTION__, strerror(err), err); + close(sk); + errno = err; + return -1; + } + + return sk; +} + +int bt_audio_service_close(int sk) +{ + return close(sk); +} + +int bt_audio_service_get_data_fd(int sk) +{ + char cmsg_b[CMSG_SPACE(sizeof(int))], m; + int err, ret; + struct iovec iov = { &m, sizeof(m) }; + struct msghdr msgh; + struct cmsghdr *cmsg; + + memset(&msgh, 0, sizeof(msgh)); + msgh.msg_iov = &iov; + msgh.msg_iovlen = 1; + msgh.msg_control = &cmsg_b; + msgh.msg_controllen = CMSG_LEN(sizeof(int)); + + ret = recvmsg(sk, &msgh, 0); + if (ret < 0) { + err = errno; + fprintf(stderr, "%s: Unable to receive fd: %s (%d)\n", + __FUNCTION__, strerror(err), err); + errno = err; + return -1; + } + + /* Receive auxiliary data in msgh */ + for (cmsg = CMSG_FIRSTHDR(&msgh); cmsg != NULL; + cmsg = CMSG_NXTHDR(&msgh, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET + && cmsg->cmsg_type == SCM_RIGHTS) + return (*(int *) CMSG_DATA(cmsg)); + } + + errno = EINVAL; + return -1; +} + +const char *bt_audio_strmsg(int type) +{ + if (type < 0 || type > (sizeof(strmsg) / sizeof(strmsg[0]))) + return NULL; + + return strmsg[type]; +} + diff --git a/src/modules/bluetooth/ipc.h b/src/modules/bluetooth/ipc.h new file mode 100644 index 00000000..c900fcd1 --- /dev/null +++ b/src/modules/bluetooth/ipc.h @@ -0,0 +1,308 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2004-2008 Marcel Holtmann + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +/* + Message sequence chart of streaming sequence for A2DP transport + + Audio daemon User + on snd_pcm_open + <--BT_GETCAPABILITIES_REQ + + BT_GETCAPABILITIES_RSP--> + + on snd_pcm_hw_params + <--BT_SETCONFIGURATION_REQ + + BT_SETCONFIGURATION_RSP--> + + on snd_pcm_prepare + <--BT_STREAMSTART_REQ + + + BT_STREAMSTART_RSP--> + + BT_STREAMFD_IND --> + + < streams data > + .......... + + on snd_pcm_drop/snd_pcm_drain + + <--BT_STREAMSTOP_REQ + + + BT_STREAMSTOP_RSP--> + + on IPC close or appl crash + + + */ + +#ifndef BT_AUDIOCLIENT_H +#define BT_AUDIOCLIENT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include +#include + +#define BT_AUDIO_IPC_PACKET_SIZE 128 +#define BT_IPC_SOCKET_NAME "\0/org/bluez/audio" + +/* Generic message header definition, except for RSP messages */ +typedef struct { + uint8_t msg_type; +} __attribute__ ((packed)) bt_audio_msg_header_t; + +/* Generic message header definition, for all RSP messages */ +typedef struct { + bt_audio_msg_header_t msg_h; + uint8_t posix_errno; +} __attribute__ ((packed)) bt_audio_rsp_msg_header_t; + +/* Messages list */ +#define BT_GETCAPABILITIES_REQ 0 +#define BT_GETCAPABILITIES_RSP 1 + +#define BT_SETCONFIGURATION_REQ 2 +#define BT_SETCONFIGURATION_RSP 3 + +#define BT_STREAMSTART_REQ 4 +#define BT_STREAMSTART_RSP 5 + +#define BT_STREAMSTOP_REQ 6 +#define BT_STREAMSTOP_RSP 7 + +#define BT_STREAMSUSPEND_IND 8 +#define BT_STREAMRESUME_IND 9 + +#define BT_CONTROL_REQ 10 +#define BT_CONTROL_RSP 11 +#define BT_CONTROL_IND 12 + +#define BT_STREAMFD_IND 13 + +/* BT_GETCAPABILITIES_REQ */ + +#define BT_CAPABILITIES_TRANSPORT_A2DP 0 +#define BT_CAPABILITIES_TRANSPORT_SCO 1 +#define BT_CAPABILITIES_TRANSPORT_ANY 2 + +#define BT_CAPABILITIES_ACCESS_MODE_READ 1 +#define BT_CAPABILITIES_ACCESS_MODE_WRITE 2 +#define BT_CAPABILITIES_ACCESS_MODE_READWRITE 3 + +#define BT_FLAG_AUTOCONNECT 1 + +struct bt_getcapabilities_req { + bt_audio_msg_header_t h; + char device[18]; /* Address of the remote Device */ + uint8_t transport; /* Requested transport */ + uint8_t flags; /* Requested flags */ +} __attribute__ ((packed)); + +/* BT_GETCAPABILITIES_RSP */ + +/** + * SBC Codec parameters as per A2DP profile 1.0 § 4.3 + */ + +#define BT_SBC_SAMPLING_FREQ_16000 (1 << 3) +#define BT_SBC_SAMPLING_FREQ_32000 (1 << 2) +#define BT_SBC_SAMPLING_FREQ_44100 (1 << 1) +#define BT_SBC_SAMPLING_FREQ_48000 1 + +#define BT_A2DP_CHANNEL_MODE_MONO (1 << 3) +#define BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL (1 << 2) +#define BT_A2DP_CHANNEL_MODE_STEREO (1 << 1) +#define BT_A2DP_CHANNEL_MODE_JOINT_STEREO 1 + +#define BT_A2DP_BLOCK_LENGTH_4 (1 << 3) +#define BT_A2DP_BLOCK_LENGTH_8 (1 << 2) +#define BT_A2DP_BLOCK_LENGTH_12 (1 << 1) +#define BT_A2DP_BLOCK_LENGTH_16 1 + +#define BT_A2DP_SUBBANDS_4 (1 << 1) +#define BT_A2DP_SUBBANDS_8 1 + +#define BT_A2DP_ALLOCATION_SNR (1 << 1) +#define BT_A2DP_ALLOCATION_LOUDNESS 1 + +#define BT_MPEG_SAMPLING_FREQ_16000 (1 << 5) +#define BT_MPEG_SAMPLING_FREQ_22050 (1 << 4) +#define BT_MPEG_SAMPLING_FREQ_24000 (1 << 3) +#define BT_MPEG_SAMPLING_FREQ_32000 (1 << 2) +#define BT_MPEG_SAMPLING_FREQ_44100 (1 << 1) +#define BT_MPEG_SAMPLING_FREQ_48000 1 + +#define BT_MPEG_LAYER_1 (1 << 2) +#define BT_MPEG_LAYER_2 (1 << 1) +#define BT_MPEG_LAYER_3 1 + +typedef struct { + uint8_t channel_mode; + uint8_t frequency; + uint8_t allocation_method; + uint8_t subbands; + uint8_t block_length; + uint8_t min_bitpool; + uint8_t max_bitpool; +} __attribute__ ((packed)) sbc_capabilities_t; + +typedef struct { + uint8_t channel_mode; + uint8_t crc; + uint8_t layer; + uint8_t frequency; + uint8_t mpf; + uint16_t bitrate; +} __attribute__ ((packed)) mpeg_capabilities_t; + +struct bt_getcapabilities_rsp { + bt_audio_rsp_msg_header_t rsp_h; + uint8_t transport; /* Granted transport */ + sbc_capabilities_t sbc_capabilities; /* A2DP only */ + mpeg_capabilities_t mpeg_capabilities; /* A2DP only */ + uint16_t sampling_rate; /* SCO only */ +} __attribute__ ((packed)); + +/* BT_SETCONFIGURATION_REQ */ +struct bt_setconfiguration_req { + bt_audio_msg_header_t h; + char device[18]; /* Address of the remote Device */ + uint8_t transport; /* Requested transport */ + uint8_t access_mode; /* Requested access mode */ + sbc_capabilities_t sbc_capabilities; /* A2DP only - only one of this field + and next one must be filled */ + mpeg_capabilities_t mpeg_capabilities; /* A2DP only */ +} __attribute__ ((packed)); + +/* BT_SETCONFIGURATION_RSP */ +struct bt_setconfiguration_rsp { + bt_audio_rsp_msg_header_t rsp_h; + uint8_t transport; /* Granted transport */ + uint8_t access_mode; /* Granted access mode */ + uint16_t link_mtu; /* Max length that transport supports */ +} __attribute__ ((packed)); + +/* BT_STREAMSTART_REQ */ +#define BT_STREAM_ACCESS_READ 0 +#define BT_STREAM_ACCESS_WRITE 1 +#define BT_STREAM_ACCESS_READWRITE 2 +struct bt_streamstart_req { + bt_audio_msg_header_t h; +} __attribute__ ((packed)); + +/* BT_STREAMSTART_RSP */ +struct bt_streamstart_rsp { + bt_audio_rsp_msg_header_t rsp_h; +} __attribute__ ((packed)); + +/* BT_STREAMFD_IND */ +/* This message is followed by one byte of data containing the stream data fd + as ancilliary data */ +struct bt_streamfd_ind { + bt_audio_msg_header_t h; +} __attribute__ ((packed)); + +/* BT_STREAMSTOP_REQ */ +struct bt_streamstop_req { + bt_audio_msg_header_t h; +} __attribute__ ((packed)); + +/* BT_STREAMSTOP_RSP */ +struct bt_streamstop_rsp { + bt_audio_rsp_msg_header_t rsp_h; +} __attribute__ ((packed)); + +/* BT_STREAMSUSPEND_IND */ +struct bt_streamsuspend_ind { + bt_audio_msg_header_t h; +} __attribute__ ((packed)); + +/* BT_STREAMRESUME_IND */ +struct bt_streamresume_ind { + bt_audio_msg_header_t h; +} __attribute__ ((packed)); + +/* BT_CONTROL_REQ */ + +#define BT_CONTROL_KEY_POWER 0x40 +#define BT_CONTROL_KEY_VOL_UP 0x41 +#define BT_CONTROL_KEY_VOL_DOWN 0x42 +#define BT_CONTROL_KEY_MUTE 0x43 +#define BT_CONTROL_KEY_PLAY 0x44 +#define BT_CONTROL_KEY_STOP 0x45 +#define BT_CONTROL_KEY_PAUSE 0x46 +#define BT_CONTROL_KEY_RECORD 0x47 +#define BT_CONTROL_KEY_REWIND 0x48 +#define BT_CONTROL_KEY_FAST_FORWARD 0x49 +#define BT_CONTROL_KEY_EJECT 0x4A +#define BT_CONTROL_KEY_FORWARD 0x4B +#define BT_CONTROL_KEY_BACKWARD 0x4C + +struct bt_control_req { + bt_audio_msg_header_t h; + uint8_t mode; /* Control Mode */ + uint8_t key; /* Control Key */ +} __attribute__ ((packed)); + +/* BT_CONTROL_RSP */ +struct bt_control_rsp { + bt_audio_rsp_msg_header_t rsp_h; + uint8_t mode; /* Control Mode */ + uint8_t key; /* Control Key */ +} __attribute__ ((packed)); + +/* BT_CONTROL_IND */ +struct bt_control_ind { + bt_audio_msg_header_t h; + uint8_t mode; /* Control Mode */ + uint8_t key; /* Control Key */ +} __attribute__ ((packed)); + +/* Function declaration */ + +/* Opens a connection to the audio service: return a socket descriptor */ +int bt_audio_service_open(); + +/* Closes a connection to the audio service */ +int bt_audio_service_close(int sk); + +/* Receives stream data file descriptor : must be called after a +BT_STREAMFD_IND message is returned */ +int bt_audio_service_get_data_fd(int sk); + +/* Human readable message type string */ +const char *bt_audio_strmsg(int type); + +#ifdef __cplusplus +} +#endif + +#endif /* BT_AUDIOCLIENT_H */ diff --git a/src/modules/bluetooth/module-bluetooth-device.c b/src/modules/bluetooth/module-bluetooth-device.c new file mode 100644 index 00000000..335a8ed5 --- /dev/null +++ b/src/modules/bluetooth/module-bluetooth-device.c @@ -0,0 +1,949 @@ +/*** + This file is part of PulseAudio. + + Copyright 2008 Joao Paulo Rechi Vita + + PulseAudio is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published + by the Free Software Foundation; either version 2 of the License, + or (at your option) any later version. + + PulseAudio is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with PulseAudio; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 + USA. +***/ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dbus-util.h" +#include "module-bluetooth-device-symdef.h" +#include "ipc.h" +#include "sbc.h" +#include "rtp.h" + +#define DEFAULT_SINK_NAME "bluetooth_sink" +#define BUFFER_SIZE 2048 +#define MAX_BITPOOL 64 +#define MIN_BITPOOL 2 +#define SOL_SCO 17 +#define SCO_TXBUFS 0x03 +#define SCO_RXBUFS 0x04 + +PA_MODULE_AUTHOR("Joao Paulo Rechi Vita"); +PA_MODULE_DESCRIPTION("Bluetooth audio sink and source"); +PA_MODULE_VERSION(PACKAGE_VERSION); +PA_MODULE_LOAD_ONCE(FALSE); +PA_MODULE_USAGE( + "name= " + "addr=
" + "profile="); + +struct bt_a2dp { + sbc_capabilities_t sbc_capabilities; + sbc_t sbc; /* Codec data */ + pa_bool_t sbc_initialized; /* Keep track if the encoder is initialized */ + int codesize; /* SBC codesize */ + int samples; /* Number of encoded samples */ + uint8_t buffer[BUFFER_SIZE]; /* Codec transfer buffer */ + int count; /* Codec transfer buffer counter */ + + uint32_t nsamples; /* Cumulative number of codec samples */ + uint16_t seq_num; /* Cumulative packet sequence */ + int frame_count; /* Current frames in buffer*/ +}; + +struct userdata { + pa_core *core; + pa_module *module; + pa_sink *sink; + + pa_thread_mq thread_mq; + pa_rtpoll *rtpoll; + pa_rtpoll_item *rtpoll_item; + pa_thread *thread; + + int64_t offset; + pa_smoother *smoother; + + pa_memchunk memchunk; + pa_mempool *mempool; + + char *name; + char *addr; + char *profile; + pa_sample_spec ss; + + int audioservice_fd; + int stream_fd; + + int transport; + char *strtransport; + int link_mtu; + size_t block_size; + pa_usec_t latency; + + struct bt_a2dp a2dp; +}; + +static const char* const valid_modargs[] = { + "name", + "addr", + "profile", + "rate", + "channels", + NULL +}; + +static int bt_audioservice_send(int sk, const bt_audio_msg_header_t *msg) { + int e; + pa_log_debug("sending %s", bt_audio_strmsg(msg->msg_type)); + if (send(sk, msg, BT_AUDIO_IPC_PACKET_SIZE, 0) > 0) + e = 0; + else { + e = -errno; + pa_log_error("Error sending data to audio service: %s(%d)", pa_cstrerror(errno), errno); + } + return e; +} + +static int bt_audioservice_recv(int sk, bt_audio_msg_header_t *inmsg) { + int e; + const char *type; + + pa_log_debug("trying to receive msg from audio service..."); + if (recv(sk, inmsg, BT_AUDIO_IPC_PACKET_SIZE, 0) > 0) { + type = bt_audio_strmsg(inmsg->msg_type); + if (type) { + pa_log_debug("Received %s", type); + e = 0; + } + else { + e = -EINVAL; + pa_log_error("Bogus message type %d received from audio service", inmsg->msg_type); + } + } + else { + e = -errno; + pa_log_error("Error receiving data from audio service: %s(%d)", pa_cstrerror(errno), errno); + } + + return e; +} + +static int bt_audioservice_expect(int sk, bt_audio_msg_header_t *rsp_hdr, int expected_type) { + int e = bt_audioservice_recv(sk, rsp_hdr); + if (e == 0) { + if (rsp_hdr->msg_type != expected_type) { + e = -EINVAL; + pa_log_error("Bogus message %s received while %s was expected", bt_audio_strmsg(rsp_hdr->msg_type), + bt_audio_strmsg(expected_type)); + } + } + return e; +} + +static int bt_getcaps(struct userdata *u) { + int e; + union { + bt_audio_rsp_msg_header_t rsp_hdr; + struct bt_getcapabilities_req getcaps_req; + struct bt_getcapabilities_rsp getcaps_rsp; + uint8_t buf[BT_AUDIO_IPC_PACKET_SIZE]; + } msg; + + memset(msg.buf, 0, BT_AUDIO_IPC_PACKET_SIZE); + msg.getcaps_req.h.msg_type = BT_GETCAPABILITIES_REQ; + strncpy(msg.getcaps_req.device, u->addr, 18); + if (strcasecmp(u->profile, "a2dp") == 0) + msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP; + else if (strcasecmp(u->profile, "hsp") == 0) + msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO; + else { + pa_log_error("invalid profile argument: %s", u->profile); + return -1; + } + msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT; + + e = bt_audioservice_send(u->audioservice_fd, &msg.getcaps_req.h); + if (e < 0) { + pa_log_error("failed to send GETCAPABILITIES_REQ"); + return e; + } + + e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp_hdr.msg_h, BT_GETCAPABILITIES_RSP); + if (e < 0) { + pa_log_error("failed to expect for GETCAPABILITIES_RSP"); + return e; + } + if (msg.rsp_hdr.posix_errno != 0) { + pa_log_error("BT_GETCAPABILITIES failed : %s (%d)", pa_cstrerror(msg.rsp_hdr.posix_errno), msg.rsp_hdr.posix_errno); + return -msg.rsp_hdr.posix_errno; + } + + if ((u->transport = msg.getcaps_rsp.transport) == BT_CAPABILITIES_TRANSPORT_A2DP) + u->a2dp.sbc_capabilities = msg.getcaps_rsp.sbc_capabilities; + + return 0; +} + +static uint8_t default_bitpool(uint8_t freq, uint8_t mode) { + switch (freq) { + case BT_SBC_SAMPLING_FREQ_16000: + case BT_SBC_SAMPLING_FREQ_32000: + return 53; + case BT_SBC_SAMPLING_FREQ_44100: + switch (mode) { + case BT_A2DP_CHANNEL_MODE_MONO: + case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: + return 31; + case BT_A2DP_CHANNEL_MODE_STEREO: + case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: + return 53; + default: + pa_log_warn("Invalid channel mode %u", mode); + return 53; + } + case BT_SBC_SAMPLING_FREQ_48000: + switch (mode) { + case BT_A2DP_CHANNEL_MODE_MONO: + case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL: + return 29; + case BT_A2DP_CHANNEL_MODE_STEREO: + case BT_A2DP_CHANNEL_MODE_JOINT_STEREO: + return 51; + default: + pa_log_warn("Invalid channel mode %u", mode); + return 51; + } + default: + pa_log_warn("Invalid sampling freq %u", freq); + return 53; + } +} + +static int bt_a2dp_init(struct userdata *u) { + sbc_capabilities_t *cap = &u->a2dp.sbc_capabilities; + unsigned int max_bitpool, min_bitpool; + + switch (u->ss.rate) { + case 48000: + cap->frequency = BT_SBC_SAMPLING_FREQ_48000; + break; + case 44100: + cap->frequency = BT_SBC_SAMPLING_FREQ_44100; + break; + case 32000: + cap->frequency = BT_SBC_SAMPLING_FREQ_32000; + break; + case 16000: + cap->frequency = BT_SBC_SAMPLING_FREQ_16000; + break; + default: + pa_log_error("Rate %d not supported", u->ss.rate); + return -1; + } + + if (u->ss.channels == 2) { + if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO) + cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO; + else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO) + cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO; + else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL) + cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL; + } else { + if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) + cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO; + } + + if (!cap->channel_mode) { + pa_log_error("No supported channel modes"); + return -1; + } + + if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16) + cap->block_length = BT_A2DP_BLOCK_LENGTH_16; + else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12) + cap->block_length = BT_A2DP_BLOCK_LENGTH_12; + else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8) + cap->block_length = BT_A2DP_BLOCK_LENGTH_8; + else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4) + cap->block_length = BT_A2DP_BLOCK_LENGTH_4; + else { + pa_log_error("No supported block lengths"); + return -1; + } + + if (cap->subbands & BT_A2DP_SUBBANDS_8) + cap->subbands = BT_A2DP_SUBBANDS_8; + else if (cap->subbands & BT_A2DP_SUBBANDS_4) + cap->subbands = BT_A2DP_SUBBANDS_4; + else { + pa_log_error("No supported subbands"); + return -1; + } + + if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS) + cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS; + else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR) + cap->allocation_method = BT_A2DP_ALLOCATION_SNR; + + min_bitpool = PA_MAX(MIN_BITPOOL, cap->min_bitpool); + max_bitpool = PA_MIN(default_bitpool(cap->frequency, cap->channel_mode), cap->max_bitpool); + + cap->min_bitpool = min_bitpool; + cap->max_bitpool = max_bitpool; + + return 0; +} + +static void bt_a2dp_setup(struct bt_a2dp *a2dp) { + sbc_capabilities_t active_capabilities = a2dp->sbc_capabilities; + + if (a2dp->sbc_initialized) + sbc_reinit(&a2dp->sbc, 0); + else + sbc_init(&a2dp->sbc, 0); + a2dp->sbc_initialized = TRUE; + + if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000) + a2dp->sbc.frequency = SBC_FREQ_16000; + + if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000) + a2dp->sbc.frequency = SBC_FREQ_32000; + + if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100) + a2dp->sbc.frequency = SBC_FREQ_44100; + + if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000) + a2dp->sbc.frequency = SBC_FREQ_48000; + + if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO) + a2dp->sbc.mode = SBC_MODE_MONO; + + if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL) + a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL; + + if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO) + a2dp->sbc.mode = SBC_MODE_STEREO; + + if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO) + a2dp->sbc.mode = SBC_MODE_JOINT_STEREO; + + a2dp->sbc.allocation = (active_capabilities.allocation_method == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR : SBC_AM_LOUDNESS); + + switch (active_capabilities.subbands) { + case BT_A2DP_SUBBANDS_4: + a2dp->sbc.subbands = SBC_SB_4; + break; + case BT_A2DP_SUBBANDS_8: + a2dp->sbc.subbands = SBC_SB_8; + break; + } + + switch (active_capabilities.block_length) { + case BT_A2DP_BLOCK_LENGTH_4: + a2dp->sbc.blocks = SBC_BLK_4; + break; + case BT_A2DP_BLOCK_LENGTH_8: + a2dp->sbc.blocks = SBC_BLK_8; + break; + case BT_A2DP_BLOCK_LENGTH_12: + a2dp->sbc.blocks = SBC_BLK_12; + break; + case BT_A2DP_BLOCK_LENGTH_16: + a2dp->sbc.blocks = SBC_BLK_16; + break; + } + + a2dp->sbc.bitpool = active_capabilities.max_bitpool; + a2dp->codesize = sbc_get_codesize(&a2dp->sbc); + a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload); +} + +static int bt_setconf(struct userdata *u) { + int e; + union { + bt_audio_rsp_msg_header_t rsp_hdr; + struct bt_setconfiguration_req setconf_req; + struct bt_setconfiguration_rsp setconf_rsp; + uint8_t buf[BT_AUDIO_IPC_PACKET_SIZE]; + } msg; + + if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { + e = bt_a2dp_init(u); + if (e < 0) { + pa_log_error("a2dp_init error"); + return e; + } + u->ss.format = PA_SAMPLE_S16LE; + } + else + u->ss.format = PA_SAMPLE_U8; + + memset(msg.buf, 0, BT_AUDIO_IPC_PACKET_SIZE); + msg.setconf_req.h.msg_type = BT_SETCONFIGURATION_REQ; + strncpy(msg.setconf_req.device, u->addr, 18); + msg.setconf_req.transport = u->transport; + if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) + msg.setconf_req.sbc_capabilities = u->a2dp.sbc_capabilities; + msg.setconf_req.access_mode = BT_CAPABILITIES_ACCESS_MODE_WRITE; + + e = bt_audioservice_send(u->audioservice_fd, &msg.setconf_req.h); + if (e < 0) { + pa_log_error("failed to send BT_SETCONFIGURATION_REQ"); + return e; + } + + e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp_hdr.msg_h, BT_SETCONFIGURATION_RSP); + if (e < 0) { + pa_log_error("failed to expect BT_SETCONFIGURATION_RSP"); + return e; + } + + if (msg.rsp_hdr.posix_errno != 0) { + pa_log_error("BT_SETCONFIGURATION failed : %s(%d)", pa_cstrerror(msg.rsp_hdr.posix_errno), msg.rsp_hdr.posix_errno); + return -msg.rsp_hdr.posix_errno; + } + + u->transport = msg.setconf_rsp.transport; + u->strtransport = (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ? pa_xstrdup("A2DP") : pa_xstrdup("SCO")); + u->link_mtu = msg.setconf_rsp.link_mtu; + + /* setup SBC encoder now we agree on parameters */ + if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { + bt_a2dp_setup(&u->a2dp); + u->block_size = u->a2dp.codesize; + pa_log_info("sbc parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n", + u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool); + } + else + u->block_size = u->link_mtu; + + return 0; +} + +static int bt_getstreamfd(struct userdata *u) { + int e; +// uint32_t period_count = io->buffer_size / io->period_size; + union { + bt_audio_rsp_msg_header_t rsp_hdr; + struct bt_streamstart_req start_req; + struct bt_streamfd_ind streamfd_ind; + uint8_t buf[BT_AUDIO_IPC_PACKET_SIZE]; + } msg; + + memset(msg.buf, 0, BT_AUDIO_IPC_PACKET_SIZE); + msg.start_req.h.msg_type = BT_STREAMSTART_REQ; + + e = bt_audioservice_send(u->audioservice_fd, &msg.start_req.h); + if (e < 0) { + pa_log_error("failed to send BT_STREAMSTART_REQ"); + return e; + } + + e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp_hdr.msg_h, BT_STREAMSTART_RSP); + if (e < 0) { + pa_log_error("failed to expect BT_STREAMSTART_RSP"); + return e; + } + + if (msg.rsp_hdr.posix_errno != 0) { + pa_log_error("BT_START failed : %s(%d)", pa_cstrerror(msg.rsp_hdr.posix_errno), msg.rsp_hdr.posix_errno); + return -msg.rsp_hdr.posix_errno; + } + + e = bt_audioservice_expect(u->audioservice_fd, &msg.streamfd_ind.h, BT_STREAMFD_IND); + if (e < 0) { + pa_log_error("failed to expect BT_STREAMFD_IND"); + return e; + } + + if (u->stream_fd >= 0) + pa_close(u->stream_fd); + + u->stream_fd = bt_audio_service_get_data_fd(u->audioservice_fd); + if (u->stream_fd < 0) { + pa_log_error("failed to get data fd: %s (%d)",pa_cstrerror(errno), errno); + return -errno; + } + + if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { + if (pa_socket_set_sndbuf(u->stream_fd, 10*u->link_mtu) < 0) { + pa_log_error("failed to set socket options for A2DP: %s (%d)",pa_cstrerror(errno), errno); + return -errno; + } + } + +// if (setsockopt(u->stream_fd, SOL_SCO, SCO_TXBUFS, &period_count, sizeof(period_count)) == 0) +// return 0; +// if (setsockopt(u->stream_fd, SOL_SCO, SO_SNDBUF, &period_count, sizeof(period_count)) == 0) +// return 0; +// /* FIXME : handle error codes */ + pa_make_fd_nonblock(u->stream_fd); +// pa_make_socket_low_delay(u->stream_fd); + + return 0; +} + +static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offset, pa_memchunk *chunk) { + struct userdata *u = PA_SINK(o)->userdata; + + pa_log_debug("got message: %d", code); + switch (code) { + + case PA_SINK_MESSAGE_SET_STATE: + switch ((pa_sink_state_t) PA_PTR_TO_UINT(data)) { + case PA_SINK_SUSPENDED: + pa_assert(PA_SINK_IS_OPENED(u->sink->thread_info.state)); + pa_smoother_pause(u->smoother, pa_rtclock_usec()); + break; + case PA_SINK_IDLE: + case PA_SINK_RUNNING: + if (u->sink->thread_info.state == PA_SINK_SUSPENDED) + pa_smoother_resume(u->smoother, pa_rtclock_usec()); + break; + case PA_SINK_UNLINKED: + case PA_SINK_INIT: + ; + } + break; + + case PA_SINK_MESSAGE_GET_LATENCY: { + pa_usec_t w, r; + r = pa_smoother_get(u->smoother, pa_rtclock_usec()); + w = pa_bytes_to_usec(u->offset + u->memchunk.length, &u->sink->sample_spec); + *((pa_usec_t*) data) = w > r ? w - r : 0; + return 0; + } + + } + + return pa_sink_process_msg(o, code, data, offset, chunk); +} + +static int sco_process_render(struct userdata *u) { + void *p; + ssize_t l; + int write_type = 0; + + u->memchunk.memblock = pa_memblock_new(u->mempool, u->block_size); + pa_log_debug("memblock asked size %d", u->block_size); + u->memchunk.length = pa_memblock_get_length(u->memchunk.memblock); + pa_log_debug("memchunk length %d", u->memchunk.length); + pa_sink_render_into_full(u->sink, &u->memchunk); + + pa_assert(u->memchunk.length > 0); + + p = pa_memblock_acquire(u->memchunk.memblock); + +sco_write: + l = pa_write(u->stream_fd, (uint8_t*) p, u->memchunk.length, &write_type); + pa_log_debug("memblock written to socket: %d bytes", l); + + pa_assert(l != 0); + + if (l < 0) { + if (errno == EINTR) { + pa_log_debug("EINTR"); + goto sco_write; + } + else if (errno == EAGAIN) { + pa_log_debug("EAGAIN"); + goto sco_write; + } + else { + pa_memblock_release(u->memchunk.memblock); + pa_memblock_unref(u->memchunk.memblock); + pa_memchunk_reset(&u->memchunk); + pa_log_debug("memchunk reseted"); + pa_log_error("Failed to write data to FIFO: %s", pa_cstrerror(errno)); + return -1; + } + } else { + pa_memblock_release(u->memchunk.memblock); + pa_memblock_unref(u->memchunk.memblock); + pa_memchunk_reset(&u->memchunk); + pa_log_debug("memchunk reseted"); + u->offset += l; + return 0; + } +} + +static int a2dp_process_render(struct userdata *u) { + ssize_t l; + int write_type = 0, written; + struct bt_a2dp *a2dp = &u->a2dp; + struct rtp_header *header = (void *) a2dp->buffer; + struct rtp_payload *payload = (void *) (a2dp->buffer + sizeof(*header)); + + pa_assert(u); + + do { + /* Render some data */ + int frame_size, encoded; + void *p; + + u->memchunk.memblock = pa_memblock_new(u->mempool, u->block_size); + pa_log_debug("memblock asked size %d", u->block_size); + u->memchunk.length = pa_memblock_get_length(u->memchunk.memblock); + pa_log_debug("memchunk length %d", u->memchunk.length); + pa_sink_render_into_full(u->sink, &u->memchunk); + + pa_assert(u->memchunk.length > 0); + + p = pa_memblock_acquire(u->memchunk.memblock); + frame_size = sbc_get_frame_length(&a2dp->sbc); + pa_log_debug("SBC frame_size: %d", frame_size); + + encoded = sbc_encode(&a2dp->sbc, (uint8_t*) p, a2dp->codesize, a2dp->buffer + a2dp->count, + sizeof(a2dp->buffer) - a2dp->count, &written); + pa_log_debug("SBC: encoded: %d; written: %d", encoded, written); + if (encoded <= 0) { + pa_log_error("SBC encoding error (%d)", encoded); + return -1; + } + pa_memblock_release(u->memchunk.memblock); + pa_memblock_unref(u->memchunk.memblock); + pa_memchunk_reset(&u->memchunk); + pa_log_debug("memchunk reseted"); + + a2dp->count += written; + a2dp->frame_count++; + a2dp->samples += encoded / frame_size; + a2dp->nsamples += encoded / frame_size; + + } while (a2dp->count + written <= u->link_mtu); + + /* write it to the fifo */ + memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload)); + payload->frame_count = a2dp->frame_count; + header->v = 2; + header->pt = 1; + header->sequence_number = htons(a2dp->seq_num); + header->timestamp = htonl(a2dp->nsamples); + header->ssrc = htonl(1); + +avdtp_write: + l = pa_write(u->stream_fd, a2dp->buffer, a2dp->count, &write_type); + pa_log_debug("avdtp_write: requested %d bytes; written %d bytes", a2dp->count, l); + + pa_assert(l != 0); + + if (l < 0) { + if (errno == EINTR) { + pa_log_debug("EINTR"); + goto avdtp_write; + } + else if (errno == EAGAIN) { + pa_log_debug("EAGAIN"); + goto avdtp_write; + } + else { + pa_log_error("Failed to write data to FIFO: %s", pa_cstrerror(errno)); + return -1; + } + } + + u->offset += a2dp->codesize*a2dp->frame_count; + + /* Reset buffer of data to send */ + a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload); + a2dp->frame_count = 0; + a2dp->samples = 0; + a2dp->seq_num++; + + return 0; +} + +static void thread_func(void *userdata) { + struct userdata *u = userdata; + + pa_assert(u); + + pa_log_debug("IO Thread starting up"); + + pa_thread_mq_install(&u->thread_mq); + pa_rtpoll_install(u->rtpoll); + + pa_smoother_set_time_offset(u->smoother, pa_rtclock_usec()); + + for (;;) { + int ret, l; + struct pollfd *pollfd; + uint64_t n; + pa_usec_t usec; + + if (PA_SINK_IS_OPENED(u->sink->thread_info.state)) { + if (u->sink->thread_info.rewind_requested) { + pa_sink_process_rewind(u->sink, 0); + } + } + + pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL); + + if (PA_SINK_IS_OPENED(u->sink->thread_info.state) && pollfd->revents) { + if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) { + if ((l = a2dp_process_render(u)) < 0) + goto fail; + } + else { + if ((l = sco_process_render(u)) < 0) + goto fail; + } + pollfd->revents = 0; + + /* feed the time smoother */ + n = u->offset; + if (ioctl(u->stream_fd, SIOCOUTQ, &l) >= 0 && l > 0) + n -= l; + usec = pa_bytes_to_usec(n, &u->sink->sample_spec); + if (usec > u->latency) + usec -= u->latency; + else + usec = 0; + pa_smoother_put(u->smoother, pa_rtclock_usec(), usec); + } + + /* Hmm, nothing to do. Let's sleep */ + pa_log_debug("IO thread going to sleep"); + pollfd->events = PA_SINK_IS_OPENED(u->sink->thread_info.state) ? POLLOUT : 0; + if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0) { + pa_log_error("rtpoll_run < 0"); + goto fail; + } + pa_log_debug("IO thread waking up"); + + if (ret == 0) { + pa_log_debug("rtpoll_run == 0"); + goto finish; + } + + pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL); + if (pollfd->revents & ~POLLOUT) { + pa_log_error("FIFO shutdown."); + goto fail; + } + } + +fail: + /* If this was no regular exit from the loop we have to continue processing messages until we receive PA_MESSAGE_SHUTDOWN */ + pa_log_debug("IO thread failed"); + pa_asyncmsgq_post(u->thread_mq.outq, PA_MSGOBJECT(u->core), PA_CORE_MESSAGE_UNLOAD_MODULE, u->module, 0, NULL, NULL); + pa_asyncmsgq_wait_for(u->thread_mq.inq, PA_MESSAGE_SHUTDOWN); + +finish: + pa_log_debug("IO thread shutting down"); +} + +int pa__init(pa_module* m) { + int e; + pa_modargs *ma; + uint32_t channels; + pa_sink_new_data data; + struct pollfd *pollfd; + struct userdata *u; + + pa_assert(m); + m->userdata = u = pa_xnew0(struct userdata, 1); + u->module = m; + u->core = m->core; + u->audioservice_fd = -1; + u->stream_fd = -1; + u->transport = -1; + u->offset = 0; + u->latency = 0; + u->a2dp.sbc_initialized = FALSE; + u->smoother = pa_smoother_new(PA_USEC_PER_SEC, PA_USEC_PER_SEC*2, TRUE, 10); + u->mempool = pa_mempool_new(FALSE); + pa_memchunk_reset(&u->memchunk); + u->rtpoll = pa_rtpoll_new(); + pa_thread_mq_init(&u->thread_mq, u->core->mainloop, u->rtpoll); + u->rtpoll_item = NULL; + + if (!(ma = pa_modargs_new(m->argument, valid_modargs))) { + pa_log_error("failed to parse module arguments"); + goto fail; + } + if (!(u->name = pa_xstrdup(pa_modargs_get_value(ma, "name", DEFAULT_SINK_NAME)))) { + pa_log_error("failed to get device name from module arguments"); + goto fail; + } + if (!(u->addr = pa_xstrdup(pa_modargs_get_value(ma, "addr", NULL)))) { + pa_log_error("failed to get device address from module arguments"); + goto fail; + } + if (!(u->profile = pa_xstrdup(pa_modargs_get_value(ma, "profile", NULL)))) { + pa_log_error("failed to get profile from module arguments"); + goto fail; + } + if (pa_modargs_get_value_u32(ma, "rate", &u->ss.rate) < 0) { + pa_log_error("failed to get rate from module arguments"); + goto fail; + } + if (pa_modargs_get_value_u32(ma, "channels", &channels) < 0) { + pa_log_error("failed to get channels from module arguments"); + goto fail; + } + u->ss.channels = (uint8_t) channels; + + /* connect to the bluez audio service */ + u->audioservice_fd = bt_audio_service_open(); + if (u->audioservice_fd <= 0) { + pa_log_error("couldn't connect to bluetooth audio service"); + goto fail; + } + pa_log_debug("connected to the bluetooth audio service"); + + /* queries device capabilities */ + e = bt_getcaps(u); + if (e < 0) { + pa_log_error("failed to get device capabilities"); + goto fail; + } + pa_log_debug("got device capabilities"); + + /* configures the connection */ + e = bt_setconf(u); + if (e < 0) { + pa_log_error("failed to set config"); + goto fail; + } + pa_log_debug("connection to the device configured"); + + /* gets the device socket */ + e = bt_getstreamfd(u); + if (e < 0) { + pa_log_error("failed to get stream fd (%d)", e); + goto fail; + } + pa_log_debug("got the device socket"); + + /* create sink */ + pa_sink_new_data_init(&data); + data.driver = __FILE__; + data.module = m; + pa_sink_new_data_set_name(&data, u->name); + pa_sink_new_data_set_sample_spec(&data, &u->ss); + pa_proplist_sets(data.proplist, PA_PROP_DEVICE_STRING, u->name); + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_DESCRIPTION, "Bluetooth %s '%s' (%s)", u->strtransport, u->name, u->addr); + pa_proplist_setf(data.proplist, "bluetooth.protocol", u->profile); + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_API, "bluez"); + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_CLASS, "sound"); + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_CONNECTOR, "bluetooth"); + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_FORM_FACTOR, "headset"); /*FIXME*/ + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_VENDOR_PRODUCT_ID, "product_id"); /*FIXME*/ + pa_proplist_setf(data.proplist, PA_PROP_DEVICE_SERIAL, "serial"); /*FIXME*/ + u->sink = pa_sink_new(m->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY); + pa_sink_new_data_done(&data); + if (!u->sink) { + pa_log_error("failed to create sink"); + goto fail; + } + u->sink->userdata = u; + u->sink->parent.process_msg = sink_process_msg; + pa_sink_set_asyncmsgq(u->sink, u->thread_mq.inq); + pa_sink_set_rtpoll(u->sink, u->rtpoll); + + u->rtpoll_item = pa_rtpoll_item_new(u->rtpoll, PA_RTPOLL_NEVER, 1); + pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL); + pollfd->fd = u->stream_fd; + pollfd->events = pollfd->revents = 0; + + /* start rt thread */ + if (!(u->thread = pa_thread_new(thread_func, u))) { + pa_log_error("failed to create IO thread"); + goto fail; + } + pa_sink_put(u->sink); + + pa_modargs_free(ma); + return 0; + +fail: + if (ma) + pa_modargs_free(ma); + pa__done(m); + return -1; +} + +void pa__done(pa_module *m) { + struct userdata *u; + pa_assert(m); + + if (!(u = m->userdata)) + return; + + if (u->sink) + pa_sink_unlink(u->sink); + + if (u->thread) { + pa_asyncmsgq_send(u->thread_mq.inq, NULL, PA_MESSAGE_SHUTDOWN, NULL, 0, NULL); + pa_thread_free(u->thread); + } + + pa_thread_mq_done(&u->thread_mq); + + if (u->sink) + pa_sink_unref(u->sink); + + if (u->rtpoll_item) + pa_rtpoll_item_free(u->rtpoll_item); + + if (u->rtpoll) + pa_rtpoll_free(u->rtpoll); + + if (u->memchunk.memblock) + pa_memblock_unref(u->memchunk.memblock); + + if (u->mempool) + pa_mempool_free(u->mempool); + + if (u->smoother) + pa_smoother_free(u->smoother); + + if (u->name) + pa_xfree(u->name); + + if (u->addr) + pa_xfree(u->addr); + + if (u->profile) + pa_xfree(u->profile); + + if (u->stream_fd >= 0) + pa_close(u->stream_fd); + + if (u->audioservice_fd >= 0) + pa_close(u->audioservice_fd); + + pa_xfree(u); +} diff --git a/src/modules/bluetooth/module-bluetooth-discover.c b/src/modules/bluetooth/module-bluetooth-discover.c new file mode 100644 index 00000000..12473dff --- /dev/null +++ b/src/modules/bluetooth/module-bluetooth-discover.c @@ -0,0 +1,532 @@ +/*** + This file is part of PulseAudio. + + Copyright 2008 Joao Paulo Rechi Vita + + PulseAudio is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published + by the Free Software Foundation; either version 2 of the License, + or (at your option) any later version. + + PulseAudio is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with PulseAudio; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 + USA. +***/ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "dbus-util.h" +#include "module-bluetooth-discover-symdef.h" + +PA_MODULE_AUTHOR("Joao Paulo Rechi Vita"); +PA_MODULE_DESCRIPTION("Detect available bluetooth audio devices and load bluetooth audio drivers"); +PA_MODULE_VERSION(PACKAGE_VERSION); +PA_MODULE_USAGE(""); + +#define HSP_HS_UUID "00001108-0000-1000-8000-00805F9B34FB" +#define HFP_HS_UUID "0000111E-0000-1000-8000-00805F9B34FB" +#define A2DP_SOURCE_UUID "0000110A-0000-1000-8000-00805F9B34FB" +#define A2DP_SINK_UUID "0000110B-0000-1000-8000-00805F9B34FB" + +struct uuid { + char *uuid; + PA_LLIST_FIELDS(struct uuid); +}; + +struct device { + char *name; + char *object_path; + int paired; + struct adapter *adapter; + char *alias; + int connected; + PA_LLIST_HEAD(struct uuid, uuid_list); + char *address; + int class; + int trusted; + PA_LLIST_FIELDS(struct device); +}; + +struct adapter { + char *object_path; + char *mode; + char *address; + PA_LLIST_HEAD(struct device, device_list); + PA_LLIST_FIELDS(struct adapter); +}; + +struct userdata { + pa_module *module; + pa_dbus_connection *conn; + PA_LLIST_HEAD(struct adapter, adapter_list); +}; + +static struct uuid *uuid_new(const char *uuid) { + struct uuid *node = pa_xnew(struct uuid, 1); + node->uuid = pa_xstrdup(uuid); + PA_LLIST_INIT(struct uuid, node); + return node; +} + +static void uuid_free(struct uuid *uuid) { + pa_xfree(uuid); +} + +static struct device *device_new(const char *device, struct adapter *adapter) { + struct device *node = pa_xnew(struct device, 1); + node->name = NULL; + node->object_path = pa_xstrdup(device); + node->paired = -1; + node->adapter = adapter; + node->alias = NULL; + node->connected = -1; + PA_LLIST_HEAD_INIT(struct uuid, node->uuid_list); + node->address = NULL; + node->class = -1; + node->trusted = -1; + PA_LLIST_INIT(struct device, node); + return node; +} + +static void device_free(struct device *device) { + struct uuid *uuid_list_i; + while (device->uuid_list) { + uuid_list_i = device->uuid_list; + PA_LLIST_REMOVE(struct uuid, device->uuid_list, uuid_list_i); + uuid_free(uuid_list_i); + } + pa_xfree(device); +} + +static struct adapter *adapter_new(const char *adapter) { + struct adapter *node = pa_xnew(struct adapter, 1); + node->object_path = pa_xstrdup(adapter); + node->mode = NULL; + node->address = NULL; + PA_LLIST_HEAD_INIT(struct device, node->device_list); + PA_LLIST_INIT(struct adapter, node); + return node; +} + +static void adapter_free(struct adapter *adapter) { + struct device *device_list_i; + while (adapter->device_list) { + device_list_i = adapter->device_list; + PA_LLIST_REMOVE(struct device, adapter->device_list, device_list_i); + device_free(device_list_i); + } + pa_xfree(adapter); +} + +static void print_devices(struct device *device_list) { + struct device *device_list_i = device_list; + while (device_list_i != NULL) { + struct uuid *uuid_list_i = device_list_i->uuid_list; + if (strcmp(device_list_i->object_path, "/DEVICE_HEAD") != 0) { + pa_log(" [ %s ]", device_list_i->object_path); + pa_log(" Name = %s", device_list_i->name); + pa_log(" Paired = %d", device_list_i->paired); + pa_log(" Adapter = %s", device_list_i->adapter->object_path); + pa_log(" Alias = %s", device_list_i->alias); + pa_log(" Connected = %d", device_list_i->connected); + pa_log(" UUIDs = "); + while (uuid_list_i != NULL) { + if (strcmp(uuid_list_i->uuid, "UUID_HEAD") != 0) + pa_log(" %s", uuid_list_i->uuid); + uuid_list_i = uuid_list_i->next; + } + pa_log(" Address = %s", device_list_i->address); + pa_log(" Class = 0x%x", device_list_i->class); + pa_log(" Trusted = %d", device_list_i->trusted); + } + device_list_i = device_list_i->next; + } +} + +static void print_adapters(struct adapter *adapter_list) { + struct adapter *adapter_list_i = adapter_list; + while (adapter_list_i != NULL) { + if (strcmp(adapter_list_i->object_path, "/ADAPTER_HEAD") != 0) { + pa_log("[ %s ]", adapter_list_i->object_path); + pa_log(" Mode = %s", adapter_list_i->mode); + pa_log(" Address = %s", adapter_list_i->address); + print_devices(adapter_list_i->device_list); + } + adapter_list_i = adapter_list_i->next; + } +} + +static void detect_adapters(struct userdata *u) { + DBusError e; + DBusMessage *m = NULL, *r = NULL; + DBusMessageIter arg_i, element_i, dict_i, variant_i; + struct adapter *adapter_list_i; + const char *key, *value; + + pa_assert(u); + dbus_error_init(&e); + + /* get adapters */ + pa_assert_se(m = dbus_message_new_method_call("org.bluez", "/", "org.bluez.Manager", "ListAdapters")); + r = dbus_connection_send_with_reply_and_block(pa_dbus_connection_get(u->conn), m, -1, &e); + if (!r) { + pa_log("org.bluez.Manager.ListAdapters failed: %s", e.message); + goto fail; + } + if (!dbus_message_iter_init(r, &arg_i)) { + pa_log("org.bluez.Manager.ListAdapters reply has no arguments"); + goto fail; + } + if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_ARRAY) { + pa_log("org.bluez.Manager.ListAdapters argument is not an array"); + goto fail; + } + dbus_message_iter_recurse(&arg_i, &element_i); + while (dbus_message_iter_get_arg_type(&element_i) != DBUS_TYPE_INVALID) { + if (dbus_message_iter_get_arg_type(&element_i) == DBUS_TYPE_OBJECT_PATH) { + struct adapter *node; + dbus_message_iter_get_basic(&element_i, &value); + node = adapter_new(value); + PA_LLIST_PREPEND(struct adapter, u->adapter_list, node); + } + dbus_message_iter_next(&element_i); + } + + /* get adapter properties */ + adapter_list_i = u->adapter_list; + while (adapter_list_i != NULL) { + pa_assert_se(m = dbus_message_new_method_call("org.bluez", adapter_list_i->object_path, "org.bluez.Adapter", "GetProperties")); + r = dbus_connection_send_with_reply_and_block(pa_dbus_connection_get(u->conn), m, -1, &e); + if (!r) { + pa_log("org.bluez.Adapter.GetProperties failed: %s", e.message); + goto fail; + } + if (!dbus_message_iter_init(r, &arg_i)) { + pa_log("org.bluez.Adapter.GetProperties reply has no arguments"); + goto fail; + } + if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_ARRAY) { + pa_log("org.bluez.Adapter.GetProperties argument is not an array"); + goto fail; + } + dbus_message_iter_recurse(&arg_i, &element_i); + while (dbus_message_iter_get_arg_type(&element_i) != DBUS_TYPE_INVALID) { + if (dbus_message_iter_get_arg_type(&element_i) == DBUS_TYPE_DICT_ENTRY) { + dbus_message_iter_recurse(&element_i, &dict_i); + dbus_message_iter_get_basic(&dict_i, &key); + dbus_message_iter_next(&dict_i); + dbus_message_iter_recurse(&dict_i, &variant_i); + dbus_message_iter_get_basic(&variant_i, &value); + if (strcmp(key, "Mode") == 0) + adapter_list_i->mode = pa_xstrdup(value); + else if (strcmp(key, "Address") == 0) + adapter_list_i->address = pa_xstrdup(value); + } + dbus_message_iter_next(&element_i); + } + adapter_list_i = adapter_list_i->next; + } + +fail: + if (m) + dbus_message_unref(m); + if (r) + dbus_message_unref(r); + dbus_error_free(&e); +} + +static void detect_devices(struct userdata *u) { + DBusError e; + DBusMessage *m = NULL, *r = NULL; + DBusMessageIter arg_i, element_i, dict_i, variant_i; + struct adapter *adapter_list_i; + struct device *device_list_i; + const char *key, *value; + unsigned int uvalue; + + pa_assert(u); + dbus_error_init(&e); + + /* get devices of each adapter */ + adapter_list_i = u->adapter_list; + while (adapter_list_i != NULL) { + pa_assert_se(m = dbus_message_new_method_call("org.bluez", adapter_list_i->object_path, "org.bluez.Adapter", "ListDevices")); + r = dbus_connection_send_with_reply_and_block(pa_dbus_connection_get(u->conn), m, -1, &e); + if (!r) { + pa_log("org.bluez.Adapter.ListDevices failed: %s", e.message); + goto fail; + } + if (!dbus_message_iter_init(r, &arg_i)) { + pa_log("org.bluez.Adapter.ListDevices reply has no arguments"); + goto fail; + } + if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_ARRAY) { + pa_log("org.bluez.Adapter.ListDevices argument is not an array"); + goto fail; + } + dbus_message_iter_recurse(&arg_i, &element_i); + while (dbus_message_iter_get_arg_type(&element_i) != DBUS_TYPE_INVALID) { + if (dbus_message_iter_get_arg_type(&element_i) == DBUS_TYPE_OBJECT_PATH) { + struct device *node; + dbus_message_iter_get_basic(&element_i, &value); + node = device_new(value, adapter_list_i); + PA_LLIST_PREPEND(struct device, adapter_list_i->device_list, node); + } + dbus_message_iter_next(&element_i); + } + adapter_list_i = adapter_list_i->next; + } + + /* get device properties */ + adapter_list_i = u->adapter_list; + while (adapter_list_i != NULL) { + device_list_i = adapter_list_i->device_list; + while (device_list_i != NULL) { + pa_assert_se(m = dbus_message_new_method_call("org.bluez", device_list_i->object_path, "org.bluez.Device", "GetProperties")); + r = dbus_connection_send_with_reply_and_block(pa_dbus_connection_get(u->conn), m, -1, &e); + if (!r) { + pa_log("org.bluez.Device.GetProperties failed: %s", e.message); + goto fail; + } + if (!dbus_message_iter_init(r, &arg_i)) { + pa_log("org.bluez.Device.GetProperties reply has no arguments"); + goto fail; + } + if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_ARRAY) { + pa_log("org.bluez.Device.GetProperties argument is not an array"); + goto fail; + } + dbus_message_iter_recurse(&arg_i, &element_i); + while (dbus_message_iter_get_arg_type(&element_i) != DBUS_TYPE_INVALID) { + if (dbus_message_iter_get_arg_type(&element_i) == DBUS_TYPE_DICT_ENTRY) { + dbus_message_iter_recurse(&element_i, &dict_i); + dbus_message_iter_get_basic(&dict_i, &key); + dbus_message_iter_next(&dict_i); + dbus_message_iter_recurse(&dict_i, &variant_i); + if (strcmp(key, "Name") == 0) { + dbus_message_iter_get_basic(&variant_i, &value); + device_list_i->name = pa_xstrdup(value); + } + else if (strcmp(key, "Paired") == 0) { + dbus_message_iter_get_basic(&variant_i, &uvalue); + device_list_i->paired = uvalue; + } + else if (strcmp(key, "Alias") == 0) { + dbus_message_iter_get_basic(&variant_i, &value); + device_list_i->alias = pa_xstrdup(value); + } + else if (strcmp(key, "Connected") == 0) { + dbus_message_iter_get_basic(&variant_i, &uvalue); + device_list_i->connected = uvalue; + } + else if (strcmp(key, "UUIDs") == 0) { + DBusMessageIter uuid_i; + pa_bool_t is_audio_device = FALSE; + dbus_message_iter_recurse(&variant_i, &uuid_i); + while (dbus_message_iter_get_arg_type(&uuid_i) != DBUS_TYPE_INVALID) { + struct uuid *node; + dbus_message_iter_get_basic(&uuid_i, &value); + node = uuid_new(value); + PA_LLIST_PREPEND(struct uuid, device_list_i->uuid_list, node); + if ( (strcasecmp(value, HSP_HS_UUID) == 0) || (strcasecmp(value, HFP_HS_UUID) == 0) || + (strcasecmp(value, A2DP_SOURCE_UUID) == 0) || (strcasecmp(value, A2DP_SINK_UUID) == 0) ) + is_audio_device = TRUE; + dbus_message_iter_next(&uuid_i); + } + if (!is_audio_device) { + /* remove the current device from the list */ + PA_LLIST_REMOVE(struct device, adapter_list_i->device_list, device_list_i); + pa_xfree(device_list_i); + break; + } + } + else if (strcmp(key, "Address") == 0) { + dbus_message_iter_get_basic(&variant_i, &value); + device_list_i->address = pa_xstrdup(value); + } + else if (strcmp(key, "Class") == 0) { + dbus_message_iter_get_basic(&variant_i, &uvalue); + device_list_i->class = uvalue; + } + else if (strcmp(key, "Trusted") == 0) { + dbus_message_iter_get_basic(&variant_i, &uvalue); + device_list_i->trusted = uvalue; + } + } + dbus_message_iter_next(&element_i); + } + device_list_i = device_list_i->next; + } + adapter_list_i = adapter_list_i->next; + } + +fail: + if (m) + dbus_message_unref(m); + if (r) + dbus_message_unref(r); + dbus_error_free(&e); +} + +static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *msg, void *userdata) { + DBusMessageIter arg_i; + DBusError err; + const char *value; + struct userdata *u; + + pa_assert(bus); + pa_assert(msg); + pa_assert(userdata); + u = userdata; + dbus_error_init(&err); + + pa_log("dbus: interface=%s, path=%s, member=%s\n", + dbus_message_get_interface(msg), + dbus_message_get_path(msg), + dbus_message_get_member(msg)); + + if (dbus_message_is_signal(msg, "org.bluez.Manager", "AdapterAdded")) { + if (!dbus_message_iter_init(msg, &arg_i)) + pa_log("dbus: message has no parameters"); + else if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_OBJECT_PATH) + pa_log("dbus: argument is not object path"); + else { + dbus_message_iter_get_basic(&arg_i, &value); + pa_log("hcid: adapter %s added", value); + } + } + else if (dbus_message_is_signal(msg, "org.bluez.Manager", "AdapterRemoved")) { + if (!dbus_message_iter_init(msg, &arg_i)) + pa_log("dbus: message has no parameters"); + else if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_OBJECT_PATH) + pa_log("dbus: argument is not object path"); + else { + dbus_message_iter_get_basic(&arg_i, &value); + pa_log("hcid: adapter %s removed", value); + } + } + else if (dbus_message_is_signal(msg, "org.bluez.Adapter", "DeviceCreated")) { + if (!dbus_message_iter_init(msg, &arg_i)) + pa_log("dbus: message has no parameters"); + else if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_OBJECT_PATH) + pa_log("dbus: argument is not object path"); + else { + dbus_message_iter_get_basic(&arg_i, &value); + pa_log("hcid: device %s created", value); + } + } + else if (dbus_message_is_signal(msg, "org.bluez.Adapter", "DeviceRemoved")) { + if (!dbus_message_iter_init(msg, &arg_i)) + pa_log("dbus: message has no parameters"); + else if (dbus_message_iter_get_arg_type(&arg_i) != DBUS_TYPE_OBJECT_PATH) + pa_log("dbus: argument is not object path"); + else { + dbus_message_iter_get_basic(&arg_i, &value); + pa_log("hcid: device %s removed", value); + } + } + + dbus_error_free(&err); + return DBUS_HANDLER_RESULT_HANDLED; +} + +void pa__done(pa_module* m) { + struct userdata *u; + struct adapter *adapter_list_i; + + pa_assert(m); + + if (!(u = m->userdata)) + return; + + while (u->adapter_list) { + adapter_list_i = u->adapter_list; + PA_LLIST_REMOVE(struct adapter, u->adapter_list, adapter_list_i); + adapter_free(adapter_list_i); + } + + pa_dbus_connection_unref(u->conn); + pa_xfree(u); + pa_log("Unloading module-bt-discover"); + return; +} + +int pa__init(pa_module* m) { + DBusError err; + struct adapter *adapter_list_i; + struct device *device_list_i; + struct userdata *u; + + pa_assert(m); + pa_log("Loading module-bt-discover"); + dbus_error_init(&err); + m->userdata = u = pa_xnew0(struct userdata, 1); + u->module = m; + PA_LLIST_HEAD_INIT(struct adapter, u->adapter_list); + + /* connect to the bus */ + u->conn = pa_dbus_bus_get(m->core, DBUS_BUS_SYSTEM, &err); + if ( dbus_error_is_set(&err) || (u->conn == NULL) ) { + pa_log("Failed to get D-Bus connection: %s", err.message); + goto fail; + } + + /* static detection of bluetooth audio devices */ + detect_adapters(u); + detect_devices(u); + + print_adapters(u->adapter_list); + + /* load device modules */ + adapter_list_i = u->adapter_list; + while (adapter_list_i != NULL) { + device_list_i = adapter_list_i->device_list; + while (device_list_i != NULL) { + pa_log("Loading module-bt-device for %s", device_list_i->name); + /* TODO: call module */ + device_list_i = device_list_i->next; + } + adapter_list_i = adapter_list_i->next; + } + + /* dynamic detection of bluetooth audio devices */ + if (!dbus_connection_add_filter(pa_dbus_connection_get(u->conn), filter_cb, u, NULL)) { + pa_log_error("Failed to add filter function"); + goto fail; + } + dbus_bus_add_match(pa_dbus_connection_get(u->conn), "type='signal',sender='org.bluez',interface='org.bluez.Manager'", &err); + if (dbus_error_is_set(&err)) { + pa_log_error("Unable to subscribe to org.bluez.Manager signals: %s: %s", err.name, err.message); + goto fail; + } + dbus_bus_add_match(pa_dbus_connection_get(u->conn), "type='signal',sender='org.bluez',interface='org.bluez.Adapter'", &err); + if (dbus_error_is_set(&err)) { + pa_log_error("Unable to subscribe to org.bluez.Adapter signals: %s: %s", err.name, err.message); + goto fail; + } + + return 0; + +fail: + dbus_error_free(&err); + pa__done(m); + return -1; +} diff --git a/src/modules/bluetooth/rtp.h b/src/modules/bluetooth/rtp.h new file mode 100644 index 00000000..690bd43a --- /dev/null +++ b/src/modules/bluetooth/rtp.h @@ -0,0 +1,76 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2004-2008 Marcel Holtmann + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#if __BYTE_ORDER == __LITTLE_ENDIAN + +struct rtp_header { + uint8_t cc:4; + uint8_t x:1; + uint8_t p:1; + uint8_t v:2; + + uint8_t pt:7; + uint8_t m:1; + + uint16_t sequence_number; + uint32_t timestamp; + uint32_t ssrc; + uint32_t csrc[0]; +} __attribute__ ((packed)); + +struct rtp_payload { + uint8_t frame_count:4; + uint8_t rfa0:1; + uint8_t is_last_fragment:1; + uint8_t is_first_fragment:1; + uint8_t is_fragmented:1; +} __attribute__ ((packed)); + +#elif __BYTE_ORDER == __BIG_ENDIAN + +struct rtp_header { + uint8_t v:2; + uint8_t p:1; + uint8_t x:1; + uint8_t cc:4; + + uint8_t m:1; + uint8_t pt:7; + + uint16_t sequence_number; + uint32_t timestamp; + uint32_t ssrc; + uint32_t csrc[0]; +} __attribute__ ((packed)); + +struct rtp_payload { + uint8_t is_fragmented:1; + uint8_t is_first_fragment:1; + uint8_t is_last_fragment:1; + uint8_t rfa0:1; + uint8_t frame_count:4; +} __attribute__ ((packed)); + +#else +#error "Unknown byte order" +#endif diff --git a/src/modules/bluetooth/sbc.c b/src/modules/bluetooth/sbc.c new file mode 100644 index 00000000..6303421f --- /dev/null +++ b/src/modules/bluetooth/sbc.c @@ -0,0 +1,1411 @@ +/* + * + * Bluetooth low-complexity, subband codec (SBC) library + * + * Copyright (C) 2004-2008 Marcel Holtmann + * Copyright (C) 2004-2005 Henryk Ploetz + * Copyright (C) 2005-2008 Brad Midgley + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +/* todo items: + + use a log2 table for byte integer scale factors calculation (sum log2 results + for high and low bytes) fill bitpool by 16 bits instead of one at a time in + bits allocation/bitpool generation port to the dsp + +*/ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include + +#include "sbc_math.h" +#include "sbc_tables.h" + +#include "sbc.h" + +#define SBC_SYNCWORD 0x9C + +/* This structure contains an unpacked SBC frame. + Yes, there is probably quite some unused space herein */ +struct sbc_frame { + uint8_t frequency; + uint8_t block_mode; + uint8_t blocks; + enum { + MONO = SBC_MODE_MONO, + DUAL_CHANNEL = SBC_MODE_DUAL_CHANNEL, + STEREO = SBC_MODE_STEREO, + JOINT_STEREO = SBC_MODE_JOINT_STEREO + } mode; + uint8_t channels; + enum { + LOUDNESS = SBC_AM_LOUDNESS, + SNR = SBC_AM_SNR + } allocation; + uint8_t subband_mode; + uint8_t subbands; + uint8_t bitpool; + uint8_t codesize; + uint8_t length; + + /* bit number x set means joint stereo has been used in subband x */ + uint8_t joint; + + /* only the lower 4 bits of every element are to be used */ + uint8_t scale_factor[2][8]; + + /* raw integer subband samples in the frame */ + + int32_t sb_sample_f[16][2][8]; + int32_t sb_sample[16][2][8]; /* modified subband samples */ + int16_t pcm_sample[2][16*8]; /* original pcm audio samples */ +}; + +struct sbc_decoder_state { + int subbands; + int32_t V[2][170]; + int offset[2][16]; +}; + +struct sbc_encoder_state { + int subbands; + int position[2]; + int32_t X[2][160]; +}; + +/* + * Calculates the CRC-8 of the first len bits in data + */ +static const uint8_t crc_table[256] = { + 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, + 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, + 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, + 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, + 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, + 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, + 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, + 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, + 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, + 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, + 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, + 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, + 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, + 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, + 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, + 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, + 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, + 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, + 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, + 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, + 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, + 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, + 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, + 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, + 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, + 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, + 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, + 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, + 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, + 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, + 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, + 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4 +}; + +static uint8_t sbc_crc8(const uint8_t *data, size_t len) +{ + uint8_t crc = 0x0f; + size_t i; + uint8_t octet; + + for (i = 0; i < len / 8; i++) + crc = crc_table[crc ^ data[i]]; + + octet = data[i]; + for (i = 0; i < len % 8; i++) { + char bit = ((octet ^ crc) & 0x80) >> 7; + + crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0); + + octet = octet << 1; + } + + return crc; +} + +/* + * Code straight from the spec to calculate the bits array + * Takes a pointer to the frame in question, a pointer to the bits array and + * the sampling frequency (as 2 bit integer) + */ +static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8]) +{ + uint8_t sf = frame->frequency; + + if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) { + int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice; + int ch, sb; + + for (ch = 0; ch < frame->channels; ch++) { + max_bitneed = 0; + if (frame->allocation == SNR) { + for (sb = 0; sb < frame->subbands; sb++) { + bitneed[ch][sb] = frame->scale_factor[ch][sb]; + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } else { + for (sb = 0; sb < frame->subbands; sb++) { + if (frame->scale_factor[ch][sb] == 0) + bitneed[ch][sb] = -5; + else { + if (frame->subbands == 4) + loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb]; + else + loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb]; + if (loudness > 0) + bitneed[ch][sb] = loudness / 2; + else + bitneed[ch][sb] = loudness; + } + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } + + bitcount = 0; + slicecount = 0; + bitslice = max_bitneed + 1; + do { + bitslice--; + bitcount += slicecount; + slicecount = 0; + for (sb = 0; sb < frame->subbands; sb++) { + if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16)) + slicecount++; + else if (bitneed[ch][sb] == bitslice + 1) + slicecount += 2; + } + } while (bitcount + slicecount < frame->bitpool); + + if (bitcount + slicecount == frame->bitpool) { + bitcount += slicecount; + bitslice--; + } + + for (sb = 0; sb < frame->subbands; sb++) { + if (bitneed[ch][sb] < bitslice + 2) + bits[ch][sb] = 0; + else { + bits[ch][sb] = bitneed[ch][sb] - bitslice; + if (bits[ch][sb] > 16) + bits[ch][sb] = 16; + } + } + + for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) { + if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) { + bits[ch][sb]++; + bitcount++; + } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) { + bits[ch][sb] = 2; + bitcount += 2; + } + } + + for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) { + if (bits[ch][sb] < 16) { + bits[ch][sb]++; + bitcount++; + } + } + + } + + } else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) { + int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice; + int ch, sb; + + max_bitneed = 0; + if (frame->allocation == SNR) { + for (ch = 0; ch < 2; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + bitneed[ch][sb] = frame->scale_factor[ch][sb]; + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } + } else { + for (ch = 0; ch < 2; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + if (frame->scale_factor[ch][sb] == 0) + bitneed[ch][sb] = -5; + else { + if (frame->subbands == 4) + loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb]; + else + loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb]; + if (loudness > 0) + bitneed[ch][sb] = loudness / 2; + else + bitneed[ch][sb] = loudness; + } + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } + } + + bitcount = 0; + slicecount = 0; + bitslice = max_bitneed + 1; + do { + bitslice--; + bitcount += slicecount; + slicecount = 0; + for (ch = 0; ch < 2; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16)) + slicecount++; + else if (bitneed[ch][sb] == bitslice + 1) + slicecount += 2; + } + } + } while (bitcount + slicecount < frame->bitpool); + + if (bitcount + slicecount == frame->bitpool) { + bitcount += slicecount; + bitslice--; + } + + for (ch = 0; ch < 2; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + if (bitneed[ch][sb] < bitslice + 2) { + bits[ch][sb] = 0; + } else { + bits[ch][sb] = bitneed[ch][sb] - bitslice; + if (bits[ch][sb] > 16) + bits[ch][sb] = 16; + } + } + } + + ch = 0; + sb = 0; + while (bitcount < frame->bitpool) { + if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) { + bits[ch][sb]++; + bitcount++; + } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) { + bits[ch][sb] = 2; + bitcount += 2; + } + if (ch == 1) { + ch = 0; + sb++; + if (sb >= frame->subbands) break; + } else + ch = 1; + } + + ch = 0; + sb = 0; + while (bitcount < frame->bitpool) { + if (bits[ch][sb] < 16) { + bits[ch][sb]++; + bitcount++; + } + if (ch == 1) { + ch = 0; + sb++; + if (sb >= frame->subbands) break; + } else + ch = 1; + } + + } + +} + +/* + * Unpacks a SBC frame at the beginning of the stream in data, + * which has at most len bytes into frame. + * Returns the length in bytes of the packed frame, or a negative + * value on error. The error codes are: + * + * -1 Data stream too short + * -2 Sync byte incorrect + * -3 CRC8 incorrect + * -4 Bitpool value out of bounds + */ +static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame, + size_t len) +{ + int consumed; + /* Will copy the parts of the header that are relevant to crc + * calculation here */ + uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + int crc_pos = 0; + int32_t temp; + + int audio_sample; + int ch, sb, blk, bit; /* channel, subband, block and bit standard + counters */ + int bits[2][8]; /* bits distribution */ + uint32_t levels[2][8]; /* levels derived from that */ + + if (len < 4) + return -1; + + if (data[0] != SBC_SYNCWORD) + return -2; + + frame->frequency = (data[1] >> 6) & 0x03; + + frame->block_mode = (data[1] >> 4) & 0x03; + switch (frame->block_mode) { + case SBC_BLK_4: + frame->blocks = 4; + break; + case SBC_BLK_8: + frame->blocks = 8; + break; + case SBC_BLK_12: + frame->blocks = 12; + break; + case SBC_BLK_16: + frame->blocks = 16; + break; + } + + frame->mode = (data[1] >> 2) & 0x03; + switch (frame->mode) { + case MONO: + frame->channels = 1; + break; + case DUAL_CHANNEL: /* fall-through */ + case STEREO: + case JOINT_STEREO: + frame->channels = 2; + break; + } + + frame->allocation = (data[1] >> 1) & 0x01; + + frame->subband_mode = (data[1] & 0x01); + frame->subbands = frame->subband_mode ? 8 : 4; + + frame->bitpool = data[2]; + + if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) && + frame->bitpool > 16 * frame->subbands) + return -4; + + if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) && + frame->bitpool > 32 * frame->subbands) + return -4; + + /* data[3] is crc, we're checking it later */ + + consumed = 32; + + crc_header[0] = data[1]; + crc_header[1] = data[2]; + crc_pos = 16; + + if (frame->mode == JOINT_STEREO) { + if (len * 8 < consumed + frame->subbands) + return -1; + + frame->joint = 0x00; + for (sb = 0; sb < frame->subbands - 1; sb++) + frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb; + if (frame->subbands == 4) + crc_header[crc_pos / 8] = data[4] & 0xf0; + else + crc_header[crc_pos / 8] = data[4]; + + consumed += frame->subbands; + crc_pos += frame->subbands; + } + + if (len * 8 < consumed + (4 * frame->subbands * frame->channels)) + return -1; + + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + /* FIXME assert(consumed % 4 == 0); */ + frame->scale_factor[ch][sb] = + (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F; + crc_header[crc_pos >> 3] |= + frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7)); + + consumed += 4; + crc_pos += 4; + } + } + + if (data[3] != sbc_crc8(crc_header, crc_pos)) + return -3; + + sbc_calculate_bits(frame, bits); + + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) + levels[ch][sb] = (1 << bits[ch][sb]) - 1; + } + + for (blk = 0; blk < frame->blocks; blk++) { + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + if (levels[ch][sb] > 0) { + audio_sample = 0; + for (bit = 0; bit < bits[ch][sb]; bit++) { + if (consumed > len * 8) + return -1; + + if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01) + audio_sample |= 1 << (bits[ch][sb] - bit - 1); + + consumed++; + } + + frame->sb_sample[blk][ch][sb] = + (((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) / + levels[ch][sb] - (1 << frame->scale_factor[ch][sb]); + } else + frame->sb_sample[blk][ch][sb] = 0; + } + } + } + + if (frame->mode == JOINT_STEREO) { + for (blk = 0; blk < frame->blocks; blk++) { + for (sb = 0; sb < frame->subbands; sb++) { + if (frame->joint & (0x01 << sb)) { + temp = frame->sb_sample[blk][0][sb] + + frame->sb_sample[blk][1][sb]; + frame->sb_sample[blk][1][sb] = + frame->sb_sample[blk][0][sb] - + frame->sb_sample[blk][1][sb]; + frame->sb_sample[blk][0][sb] = temp; + } + } + } + } + + if ((consumed & 0x7) != 0) + consumed += 8 - (consumed & 0x7); + + return consumed >> 3; +} + +static void sbc_decoder_init(struct sbc_decoder_state *state, + const struct sbc_frame *frame) +{ + int i, ch; + + memset(state->V, 0, sizeof(state->V)); + state->subbands = frame->subbands; + + for (ch = 0; ch < 2; ch++) + for (i = 0; i < frame->subbands * 2; i++) + state->offset[ch][i] = (10 * i + 10); +} + +static inline void sbc_synthesize_four(struct sbc_decoder_state *state, + struct sbc_frame *frame, int ch, int blk) +{ + int i, k, idx; + int32_t *v = state->V[ch]; + int *offset = state->offset[ch]; + + for (i = 0; i < 8; i++) { + /* Shifting */ + offset[i]--; + if (offset[i] < 0) { + offset[i] = 79; + memcpy(v + 80, v, 9 * sizeof(*v)); + } + + /* Distribute the new matrix value to the shifted position */ + v[offset[i]] = SCALE4_STAGED1( + MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0], + MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1], + MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2], + MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3]))))); + } + + /* Compute the samples */ + for (idx = 0, i = 0; i < 4; i++, idx += 5) { + k = (i + 4) & 0xf; + + /* Store in output, Q0 */ + frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED2( + MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0], + MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0], + MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1], + MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1], + MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2], + MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2], + MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3], + MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3], + MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4], + MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4]))))))))))); + } +} + +static inline void sbc_synthesize_eight(struct sbc_decoder_state *state, + struct sbc_frame *frame, int ch, int blk) +{ + int i, j, k, idx; + int *offset = state->offset[ch]; + + for (i = 0; i < 16; i++) { + /* Shifting */ + offset[i]--; + if (offset[i] < 0) { + offset[i] = 159; + for (j = 0; j < 9; j++) + state->V[ch][j + 160] = state->V[ch][j]; + } + + /* Distribute the new matrix value to the shifted position */ + state->V[ch][offset[i]] = SCALE8_STAGED1( + MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0], + MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1], + MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2], + MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3], + MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4], + MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5], + MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6], + MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7]))))))))); + } + + /* Compute the samples */ + for (idx = 0, i = 0; i < 8; i++, idx += 5) { + k = (i + 8) & 0xf; + + /* Store in output */ + frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED2( // Q0 + MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0], + MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0], + MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1], + MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1], + MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2], + MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2], + MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3], + MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3], + MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4], + MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4]))))))))))); + } +} + +static int sbc_synthesize_audio(struct sbc_decoder_state *state, + struct sbc_frame *frame) +{ + int ch, blk; + + switch (frame->subbands) { + case 4: + for (ch = 0; ch < frame->channels; ch++) { + for (blk = 0; blk < frame->blocks; blk++) + sbc_synthesize_four(state, frame, ch, blk); + } + return frame->blocks * 4; + + case 8: + for (ch = 0; ch < frame->channels; ch++) { + for (blk = 0; blk < frame->blocks; blk++) + sbc_synthesize_eight(state, frame, ch, blk); + } + return frame->blocks * 8; + + default: + return -EIO; + } +} + +static void sbc_encoder_init(struct sbc_encoder_state *state, + const struct sbc_frame *frame) +{ + memset(&state->X, 0, sizeof(state->X)); + state->subbands = frame->subbands; + state->position[0] = state->position[1] = 9 * frame->subbands; +} + +static inline void _sbc_analyze_four(const int32_t *in, int32_t *out) +{ + sbc_fixed_t t[8], s[5]; + + t[0] = SCALE4_STAGE1( /* Q8 */ + MULA(_sbc_proto_4[0], in[8] - in[32], /* Q18 */ + MUL( _sbc_proto_4[1], in[16] - in[24]))); + + t[1] = SCALE4_STAGE1( + MULA(_sbc_proto_4[2], in[1], + MULA(_sbc_proto_4[3], in[9], + MULA(_sbc_proto_4[4], in[17], + MULA(_sbc_proto_4[5], in[25], + MUL( _sbc_proto_4[6], in[33])))))); + + t[2] = SCALE4_STAGE1( + MULA(_sbc_proto_4[7], in[2], + MULA(_sbc_proto_4[8], in[10], + MULA(_sbc_proto_4[9], in[18], + MULA(_sbc_proto_4[10], in[26], + MUL( _sbc_proto_4[11], in[34])))))); + + t[3] = SCALE4_STAGE1( + MULA(_sbc_proto_4[12], in[3], + MULA(_sbc_proto_4[13], in[11], + MULA(_sbc_proto_4[14], in[19], + MULA(_sbc_proto_4[15], in[27], + MUL( _sbc_proto_4[16], in[35])))))); + + t[4] = SCALE4_STAGE1( + MULA(_sbc_proto_4[17], in[4] + in[36], + MULA(_sbc_proto_4[18], in[12] + in[28], + MUL( _sbc_proto_4[19], in[20])))); + + t[5] = SCALE4_STAGE1( + MULA(_sbc_proto_4[16], in[5], + MULA(_sbc_proto_4[15], in[13], + MULA(_sbc_proto_4[14], in[21], + MULA(_sbc_proto_4[13], in[29], + MUL( _sbc_proto_4[12], in[37])))))); + + /* don't compute t[6]... this term always multiplies + * with cos(pi/2) = 0 */ + + t[7] = SCALE4_STAGE1( + MULA(_sbc_proto_4[6], in[7], + MULA(_sbc_proto_4[5], in[15], + MULA(_sbc_proto_4[4], in[23], + MULA(_sbc_proto_4[3], in[31], + MUL( _sbc_proto_4[2], in[39])))))); + + s[0] = MUL( _anamatrix4[0], t[0] + t[4]); + s[1] = MUL( _anamatrix4[2], t[2]); + s[2] = MULA(_anamatrix4[1], t[1] + t[3], + MUL(_anamatrix4[3], t[5])); + s[3] = MULA(_anamatrix4[3], t[1] + t[3], + MUL(_anamatrix4[1], -t[5] + t[7])); + s[4] = MUL( _anamatrix4[3], t[7]); + + out[0] = SCALE4_STAGE2( s[0] + s[1] + s[2] + s[4]); /* Q0 */ + out[1] = SCALE4_STAGE2(-s[0] + s[1] + s[3]); + out[2] = SCALE4_STAGE2(-s[0] + s[1] - s[3]); + out[3] = SCALE4_STAGE2( s[0] + s[1] - s[2] - s[4]); +} + +static inline void sbc_analyze_four(struct sbc_encoder_state *state, + struct sbc_frame *frame, int ch, int blk) +{ + int32_t *x = &state->X[ch][state->position[ch]]; + int16_t *pcm = &frame->pcm_sample[ch][blk * 4]; + + /* Input 4 Audio Samples */ + x[40] = x[0] = pcm[3]; + x[41] = x[1] = pcm[2]; + x[42] = x[2] = pcm[1]; + x[43] = x[3] = pcm[0]; + + _sbc_analyze_four(x, frame->sb_sample_f[blk][ch]); + + state->position[ch] -= 4; + if (state->position[ch] < 0) + state->position[ch] = 36; +} + +static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out) +{ + sbc_fixed_t t[8], s[8]; + + t[0] = SCALE8_STAGE1( /* Q10 */ + MULA(_sbc_proto_8[0], (in[16] - in[64]), /* Q18 = Q18 * Q0 */ + MULA(_sbc_proto_8[1], (in[32] - in[48]), + MULA(_sbc_proto_8[2], in[4], + MULA(_sbc_proto_8[3], in[20], + MULA(_sbc_proto_8[4], in[36], + MUL( _sbc_proto_8[5], in[52]))))))); + + t[1] = SCALE8_STAGE1( + MULA(_sbc_proto_8[6], in[2], + MULA(_sbc_proto_8[7], in[18], + MULA(_sbc_proto_8[8], in[34], + MULA(_sbc_proto_8[9], in[50], + MUL(_sbc_proto_8[10], in[66])))))); + + t[2] = SCALE8_STAGE1( + MULA(_sbc_proto_8[11], in[1], + MULA(_sbc_proto_8[12], in[17], + MULA(_sbc_proto_8[13], in[33], + MULA(_sbc_proto_8[14], in[49], + MULA(_sbc_proto_8[15], in[65], + MULA(_sbc_proto_8[16], in[3], + MULA(_sbc_proto_8[17], in[19], + MULA(_sbc_proto_8[18], in[35], + MULA(_sbc_proto_8[19], in[51], + MUL( _sbc_proto_8[20], in[67]))))))))))); + + t[3] = SCALE8_STAGE1( + MULA( _sbc_proto_8[21], in[5], + MULA( _sbc_proto_8[22], in[21], + MULA( _sbc_proto_8[23], in[37], + MULA( _sbc_proto_8[24], in[53], + MULA( _sbc_proto_8[25], in[69], + MULA(-_sbc_proto_8[15], in[15], + MULA(-_sbc_proto_8[14], in[31], + MULA(-_sbc_proto_8[13], in[47], + MULA(-_sbc_proto_8[12], in[63], + MUL( -_sbc_proto_8[11], in[79]))))))))))); + + t[4] = SCALE8_STAGE1( + MULA( _sbc_proto_8[26], in[6], + MULA( _sbc_proto_8[27], in[22], + MULA( _sbc_proto_8[28], in[38], + MULA( _sbc_proto_8[29], in[54], + MULA( _sbc_proto_8[30], in[70], + MULA(-_sbc_proto_8[10], in[14], + MULA(-_sbc_proto_8[9], in[30], + MULA(-_sbc_proto_8[8], in[46], + MULA(-_sbc_proto_8[7], in[62], + MUL( -_sbc_proto_8[6], in[78]))))))))))); + + t[5] = SCALE8_STAGE1( + MULA( _sbc_proto_8[31], in[7], + MULA( _sbc_proto_8[32], in[23], + MULA( _sbc_proto_8[33], in[39], + MULA( _sbc_proto_8[34], in[55], + MULA( _sbc_proto_8[35], in[71], + MULA(-_sbc_proto_8[20], in[13], + MULA(-_sbc_proto_8[19], in[29], + MULA(-_sbc_proto_8[18], in[45], + MULA(-_sbc_proto_8[17], in[61], + MUL( -_sbc_proto_8[16], in[77]))))))))))); + + t[6] = SCALE8_STAGE1( + MULA( _sbc_proto_8[36], (in[8] + in[72]), + MULA( _sbc_proto_8[37], (in[24] + in[56]), + MULA( _sbc_proto_8[38], in[40], + MULA(-_sbc_proto_8[39], in[12], + MULA(-_sbc_proto_8[5], in[28], + MULA(-_sbc_proto_8[4], in[44], + MULA(-_sbc_proto_8[3], in[60], + MUL( -_sbc_proto_8[2], in[76]))))))))); + + t[7] = SCALE8_STAGE1( + MULA( _sbc_proto_8[35], in[9], + MULA( _sbc_proto_8[34], in[25], + MULA( _sbc_proto_8[33], in[41], + MULA( _sbc_proto_8[32], in[57], + MULA( _sbc_proto_8[31], in[73], + MULA(-_sbc_proto_8[25], in[11], + MULA(-_sbc_proto_8[24], in[27], + MULA(-_sbc_proto_8[23], in[43], + MULA(-_sbc_proto_8[22], in[59], + MUL( -_sbc_proto_8[21], in[75]))))))))))); + + s[0] = MULA( _anamatrix8[0], t[0], + MUL( _anamatrix8[1], t[6])); + s[1] = MUL( _anamatrix8[7], t[1]); + s[2] = MULA( _anamatrix8[2], t[2], + MULA( _anamatrix8[3], t[3], + MULA( _anamatrix8[4], t[5], + MUL( _anamatrix8[5], t[7])))); + s[3] = MUL( _anamatrix8[6], t[4]); + s[4] = MULA( _anamatrix8[3], t[2], + MULA(-_anamatrix8[5], t[3], + MULA(-_anamatrix8[2], t[5], + MUL( -_anamatrix8[4], t[7])))); + s[5] = MULA( _anamatrix8[4], t[2], + MULA(-_anamatrix8[2], t[3], + MULA( _anamatrix8[5], t[5], + MUL( _anamatrix8[3], t[7])))); + s[6] = MULA( _anamatrix8[1], t[0], + MUL( -_anamatrix8[0], t[6])); + s[7] = MULA( _anamatrix8[5], t[2], + MULA(-_anamatrix8[4], t[3], + MULA( _anamatrix8[3], t[5], + MUL( -_anamatrix8[2], t[7])))); + + out[0] = SCALE8_STAGE2( s[0] + s[1] + s[2] + s[3]); + out[1] = SCALE8_STAGE2( s[1] - s[3] + s[4] + s[6]); + out[2] = SCALE8_STAGE2( s[1] - s[3] + s[5] - s[6]); + out[3] = SCALE8_STAGE2(-s[0] + s[1] + s[3] + s[7]); + out[4] = SCALE8_STAGE2(-s[0] + s[1] + s[3] - s[7]); + out[5] = SCALE8_STAGE2( s[1] - s[3] - s[5] - s[6]); + out[6] = SCALE8_STAGE2( s[1] - s[3] - s[4] + s[6]); + out[7] = SCALE8_STAGE2( s[0] + s[1] - s[2] + s[3]); +} + +static inline void sbc_analyze_eight(struct sbc_encoder_state *state, + struct sbc_frame *frame, int ch, + int blk) +{ + int32_t *x = &state->X[ch][state->position[ch]]; + int16_t *pcm = &frame->pcm_sample[ch][blk * 8]; + + /* Input 8 Audio Samples */ + x[80] = x[0] = pcm[7]; + x[81] = x[1] = pcm[6]; + x[82] = x[2] = pcm[5]; + x[83] = x[3] = pcm[4]; + x[84] = x[4] = pcm[3]; + x[85] = x[5] = pcm[2]; + x[86] = x[6] = pcm[1]; + x[87] = x[7] = pcm[0]; + + _sbc_analyze_eight(x, frame->sb_sample_f[blk][ch]); + + state->position[ch] -= 8; + if (state->position[ch] < 0) + state->position[ch] = 72; +} + +static int sbc_analyze_audio(struct sbc_encoder_state *state, + struct sbc_frame *frame) +{ + int ch, blk; + + switch (frame->subbands) { + case 4: + for (ch = 0; ch < frame->channels; ch++) + for (blk = 0; blk < frame->blocks; blk++) + sbc_analyze_four(state, frame, ch, blk); + return frame->blocks * 4; + + case 8: + for (ch = 0; ch < frame->channels; ch++) + for (blk = 0; blk < frame->blocks; blk++) + sbc_analyze_eight(state, frame, ch, blk); + return frame->blocks * 8; + + default: + return -EIO; + } +} + +/* + * Packs the SBC frame from frame into the memory at data. At most len + * bytes will be used, should more memory be needed an appropriate + * error code will be returned. Returns the length of the packed frame + * on success or a negative value on error. + * + * The error codes are: + * -1 Not enough memory reserved + * -2 Unsupported sampling rate + * -3 Unsupported number of blocks + * -4 Unsupported number of subbands + * -5 Bitpool value out of bounds + * -99 not implemented + */ + +static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len) +{ + int produced; + /* Will copy the header parts for CRC-8 calculation here */ + uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + int crc_pos = 0; + + uint16_t audio_sample; + + int ch, sb, blk, bit; /* channel, subband, block and bit counters */ + int bits[2][8]; /* bits distribution */ + int levels[2][8]; /* levels are derived from that */ + + u_int32_t scalefactor[2][8]; /* derived from frame->scale_factor */ + + data[0] = SBC_SYNCWORD; + + data[1] = (frame->frequency & 0x03) << 6; + + data[1] |= (frame->block_mode & 0x03) << 4; + + data[1] |= (frame->mode & 0x03) << 2; + + data[1] |= (frame->allocation & 0x01) << 1; + + switch (frame->subbands) { + case 4: + /* Nothing to do */ + break; + case 8: + data[1] |= 0x01; + break; + default: + return -4; + break; + } + + data[2] = frame->bitpool; + + if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) && + frame->bitpool > frame->subbands << 4) + return -5; + + if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) && + frame->bitpool > frame->subbands << 5) + return -5; + + /* Can't fill in crc yet */ + + produced = 32; + + crc_header[0] = data[1]; + crc_header[1] = data[2]; + crc_pos = 16; + + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + frame->scale_factor[ch][sb] = 0; + scalefactor[ch][sb] = 2; + for (blk = 0; blk < frame->blocks; blk++) { + while (scalefactor[ch][sb] < fabs(frame->sb_sample_f[blk][ch][sb])) { + frame->scale_factor[ch][sb]++; + scalefactor[ch][sb] *= 2; + } + } + } + } + + if (frame->mode == JOINT_STEREO) { + /* like frame->sb_sample but joint stereo */ + int32_t sb_sample_j[16][2]; + /* scalefactor and scale_factor in joint case */ + u_int32_t scalefactor_j[2]; + uint8_t scale_factor_j[2]; + + frame->joint = 0; + + for (sb = 0; sb < frame->subbands - 1; sb++) { + scale_factor_j[0] = 0; + scalefactor_j[0] = 2; + scale_factor_j[1] = 0; + scalefactor_j[1] = 2; + + for (blk = 0; blk < frame->blocks; blk++) { + /* Calculate joint stereo signal */ + sb_sample_j[blk][0] = + (frame->sb_sample_f[blk][0][sb] + + frame->sb_sample_f[blk][1][sb]) >> 1; + sb_sample_j[blk][1] = + (frame->sb_sample_f[blk][0][sb] - + frame->sb_sample_f[blk][1][sb]) >> 1; + + /* calculate scale_factor_j and scalefactor_j for joint case */ + while (scalefactor_j[0] < fabs(sb_sample_j[blk][0])) { + scale_factor_j[0]++; + scalefactor_j[0] *= 2; + } + while (scalefactor_j[1] < fabs(sb_sample_j[blk][1])) { + scale_factor_j[1]++; + scalefactor_j[1] *= 2; + } + } + + /* decide whether to join this subband */ + if ((scalefactor[0][sb] + scalefactor[1][sb]) > + (scalefactor_j[0] + scalefactor_j[1]) ) { + /* use joint stereo for this subband */ + frame->joint |= 1 << sb; + frame->scale_factor[0][sb] = scale_factor_j[0]; + frame->scale_factor[1][sb] = scale_factor_j[1]; + scalefactor[0][sb] = scalefactor_j[0]; + scalefactor[1][sb] = scalefactor_j[1]; + for (blk = 0; blk < frame->blocks; blk++) { + frame->sb_sample_f[blk][0][sb] = + sb_sample_j[blk][0]; + frame->sb_sample_f[blk][1][sb] = + sb_sample_j[blk][1]; + } + } + } + + data[4] = 0; + for (sb = 0; sb < frame->subbands - 1; sb++) + data[4] |= ((frame->joint >> sb) & 0x01) << (frame->subbands - 1 - sb); + + crc_header[crc_pos >> 3] = data[4]; + + produced += frame->subbands; + crc_pos += frame->subbands; + } + + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + data[produced >> 3] <<= 4; + crc_header[crc_pos >> 3] <<= 4; + data[produced >> 3] |= frame->scale_factor[ch][sb] & 0x0F; + crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F; + + produced += 4; + crc_pos += 4; + } + } + + /* align the last crc byte */ + if (crc_pos % 8) + crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8); + + data[3] = sbc_crc8(crc_header, crc_pos); + + sbc_calculate_bits(frame, bits); + + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) + levels[ch][sb] = (1 << bits[ch][sb]) - 1; + } + + for (blk = 0; blk < frame->blocks; blk++) { + for (ch = 0; ch < frame->channels; ch++) { + for (sb = 0; sb < frame->subbands; sb++) { + if (levels[ch][sb] > 0) { + audio_sample = + (uint16_t) ((((frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >> + (frame->scale_factor[ch][sb] + 1)) + + levels[ch][sb]) >> 1); + audio_sample <<= 16 - bits[ch][sb]; + for (bit = 0; bit < bits[ch][sb]; bit++) { + data[produced >> 3] <<= 1; + if (audio_sample & 0x8000) + data[produced >> 3] |= 0x1; + audio_sample <<= 1; + produced++; + } + } + } + } + } + + /* align the last byte */ + if (produced % 8) { + data[produced >> 3] <<= 8 - (produced % 8); + } + + return (produced + 7) >> 3; +} + +struct sbc_priv { + int init; + struct sbc_frame frame; + struct sbc_decoder_state dec_state; + struct sbc_encoder_state enc_state; +}; + +static void sbc_set_defaults(sbc_t *sbc, unsigned long flags) +{ + sbc->frequency = SBC_FREQ_44100; + sbc->mode = SBC_MODE_STEREO; + sbc->subbands = SBC_SB_8; + sbc->blocks = SBC_BLK_16; + sbc->bitpool = 32; +#if __BYTE_ORDER == __LITTLE_ENDIAN + sbc->endian = SBC_LE; +#elif __BYTE_ORDER == __BIG_ENDIAN + sbc->endian = SBC_BE; +#else +#error "Unknown byte order" +#endif +} + +int sbc_init(sbc_t *sbc, unsigned long flags) +{ + if (!sbc) + return -EIO; + + memset(sbc, 0, sizeof(sbc_t)); + + sbc->priv = malloc(sizeof(struct sbc_priv)); + if (!sbc->priv) + return -ENOMEM; + + memset(sbc->priv, 0, sizeof(struct sbc_priv)); + + sbc_set_defaults(sbc, flags); + + return 0; +} + +int sbc_parse(sbc_t *sbc, void *input, int input_len) +{ + return sbc_decode(sbc, input, input_len, NULL, 0, NULL); +} + +int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output, + int output_len, int *written) +{ + struct sbc_priv *priv; + char *ptr; + int i, ch, framelen, samples; + + if (!sbc && !input) + return -EIO; + + priv = sbc->priv; + + framelen = sbc_unpack_frame(input, &priv->frame, input_len); + + if (!priv->init) { + sbc_decoder_init(&priv->dec_state, &priv->frame); + priv->init = 1; + + sbc->frequency = priv->frame.frequency; + sbc->mode = priv->frame.mode; + sbc->subbands = priv->frame.subband_mode; + sbc->blocks = priv->frame.block_mode; + sbc->allocation = priv->frame.allocation; + sbc->bitpool = priv->frame.bitpool; + + priv->frame.codesize = sbc_get_codesize(sbc); + priv->frame.length = sbc_get_frame_length(sbc); + } + + if (!output) + return framelen; + + if (written) + *written = 0; + + samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame); + + ptr = output; + + if (output_len < samples * priv->frame.channels * 2) + samples = output_len / (priv->frame.channels * 2); + + for (i = 0; i < samples; i++) { + for (ch = 0; ch < priv->frame.channels; ch++) { + int16_t s; + s = priv->frame.pcm_sample[ch][i]; + +#if __BYTE_ORDER == __LITTLE_ENDIAN + if (sbc->endian == SBC_BE) { +#elif __BYTE_ORDER == __BIG_ENDIAN + if (sbc->endian == SBC_LE) { +#else +#error "Unknown byte order" +#endif + *ptr++ = (s & 0xff00) >> 8; + *ptr++ = (s & 0x00ff); + } else { + *ptr++ = (s & 0x00ff); + *ptr++ = (s & 0xff00) >> 8; + } + } + } + + if (written) + *written = samples * priv->frame.channels * 2; + + return framelen; +} + +int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output, + int output_len, int *written) +{ + struct sbc_priv *priv; + char *ptr; + int i, ch, framelen, samples; + + if (!sbc && !input) + return -EIO; + + priv = sbc->priv; + + if (written) + *written = 0; + + if (!priv->init) { + priv->frame.frequency = sbc->frequency; + priv->frame.mode = sbc->mode; + priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; + priv->frame.allocation = sbc->allocation; + priv->frame.subband_mode = sbc->subbands; + priv->frame.subbands = sbc->subbands ? 8 : 4; + priv->frame.block_mode = sbc->blocks; + priv->frame.blocks = 4 + (sbc->blocks * 4); + priv->frame.bitpool = sbc->bitpool; + priv->frame.codesize = sbc_get_codesize(sbc); + priv->frame.length = sbc_get_frame_length(sbc); + + sbc_encoder_init(&priv->enc_state, &priv->frame); + priv->init = 1; + } + + /* input must be large enough to encode a complete frame */ + if (input_len < priv->frame.codesize) + return 0; + + /* output must be large enough to receive the encoded frame */ + if (!output || output_len < priv->frame.length) + return -ENOSPC; + + ptr = input; + + for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) { + for (ch = 0; ch < priv->frame.channels; ch++) { + int16_t s; +#if __BYTE_ORDER == __LITTLE_ENDIAN + if (sbc->endian == SBC_BE) +#elif __BYTE_ORDER == __BIG_ENDIAN + if (sbc->endian == SBC_LE) +#else +#error "Unknown byte order" +#endif + s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff); + else + s = (ptr[0] & 0xff) | (ptr[1] & 0xff) << 8; + ptr += 2; + priv->frame.pcm_sample[ch][i] = s; + } + } + + samples = sbc_analyze_audio(&priv->enc_state, &priv->frame); + + framelen = sbc_pack_frame(output, &priv->frame, output_len); + + if (written) + *written = framelen; + + return samples * priv->frame.channels * 2; +} + +void sbc_finish(sbc_t *sbc) +{ + if (!sbc) + return; + + if (sbc->priv) + free(sbc->priv); + + memset(sbc, 0, sizeof(sbc_t)); +} + +int sbc_get_frame_length(sbc_t *sbc) +{ + int ret; + uint8_t subbands, channels, blocks, joint; + struct sbc_priv *priv; + + priv = sbc->priv; + if (!priv->init) { + subbands = sbc->subbands ? 8 : 4; + blocks = 4 + (sbc->blocks * 4); + channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; + joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0; + } else { + subbands = priv->frame.subbands; + blocks = priv->frame.blocks; + channels = priv->frame.channels; + joint = priv->frame.joint; + } + + ret = 4 + (4 * subbands * channels) / 8; + + /* This term is not always evenly divide so we round it up */ + if (channels == 1) + ret += ((blocks * channels * sbc->bitpool) + 7) / 8; + else + ret += (((joint ? subbands : 0) + blocks * sbc->bitpool) + 7) + / 8; + + return ret; +} + +int sbc_get_frame_duration(sbc_t *sbc) +{ + uint8_t subbands, blocks; + uint16_t frequency; + struct sbc_priv *priv; + + priv = sbc->priv; + if (!priv->init) { + subbands = sbc->subbands ? 8 : 4; + blocks = 4 + (sbc->blocks * 4); + } else { + subbands = priv->frame.subbands; + blocks = priv->frame.blocks; + } + + switch (sbc->frequency) { + case SBC_FREQ_16000: + frequency = 16000; + break; + + case SBC_FREQ_32000: + frequency = 32000; + break; + + case SBC_FREQ_44100: + frequency = 44100; + break; + + case SBC_FREQ_48000: + frequency = 48000; + break; + default: + return 0; + } + + return (1000000 * blocks * subbands) / frequency; +} + +int sbc_get_codesize(sbc_t *sbc) +{ + uint8_t subbands, channels, blocks; + struct sbc_priv *priv; + + priv = sbc->priv; + if (!priv->init) { + subbands = sbc->subbands ? 8 : 4; + blocks = 4 + (sbc->blocks * 4); + channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; + } else { + subbands = priv->frame.subbands; + blocks = priv->frame.blocks; + channels = priv->frame.channels; + } + + return subbands * blocks * channels * 2; +} + +int sbc_reinit(sbc_t *sbc, unsigned long flags) +{ + struct sbc_priv *priv; + + if (!sbc || !sbc->priv) + return -EIO; + + priv = sbc->priv; + + if (priv->init == 1) + memset(sbc->priv, 0, sizeof(struct sbc_priv)); + + sbc_set_defaults(sbc, flags); + + return 0; +} diff --git a/src/modules/bluetooth/sbc.h b/src/modules/bluetooth/sbc.h new file mode 100644 index 00000000..ab47e329 --- /dev/null +++ b/src/modules/bluetooth/sbc.h @@ -0,0 +1,97 @@ +/* + * + * Bluetooth low-complexity, subband codec (SBC) library + * + * Copyright (C) 2004-2008 Marcel Holtmann + * Copyright (C) 2004-2005 Henryk Ploetz + * Copyright (C) 2005-2006 Brad Midgley + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef __SBC_H +#define __SBC_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* sampling frequency */ +#define SBC_FREQ_16000 0x00 +#define SBC_FREQ_32000 0x01 +#define SBC_FREQ_44100 0x02 +#define SBC_FREQ_48000 0x03 + +/* blocks */ +#define SBC_BLK_4 0x00 +#define SBC_BLK_8 0x01 +#define SBC_BLK_12 0x02 +#define SBC_BLK_16 0x03 + +/* channel mode */ +#define SBC_MODE_MONO 0x00 +#define SBC_MODE_DUAL_CHANNEL 0x01 +#define SBC_MODE_STEREO 0x02 +#define SBC_MODE_JOINT_STEREO 0x03 + +/* allocation method */ +#define SBC_AM_LOUDNESS 0x00 +#define SBC_AM_SNR 0x01 + +/* subbands */ +#define SBC_SB_4 0x00 +#define SBC_SB_8 0x01 + +/* Data endianess */ +#define SBC_LE 0x00 +#define SBC_BE 0x01 + +struct sbc_struct { + unsigned long flags; + + uint8_t frequency; + uint8_t blocks; + uint8_t subbands; + uint8_t mode; + uint8_t allocation; + uint8_t bitpool; + uint8_t endian; + + void *priv; +}; + +typedef struct sbc_struct sbc_t; + +int sbc_init(sbc_t *sbc, unsigned long flags); +int sbc_reinit(sbc_t *sbc, unsigned long flags); +int sbc_parse(sbc_t *sbc, void *input, int input_len); +int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output, + int output_len, int *len); +int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output, + int output_len, int *written); +int sbc_get_frame_length(sbc_t *sbc); +int sbc_get_frame_duration(sbc_t *sbc); +int sbc_get_codesize(sbc_t *sbc); +void sbc_finish(sbc_t *sbc); + +#ifdef __cplusplus +} +#endif + +#endif /* __SBC_H */ diff --git a/src/modules/bluetooth/sbc_math.h b/src/modules/bluetooth/sbc_math.h new file mode 100644 index 00000000..b3d87a62 --- /dev/null +++ b/src/modules/bluetooth/sbc_math.h @@ -0,0 +1,72 @@ +/* + * + * Bluetooth low-complexity, subband codec (SBC) library + * + * Copyright (C) 2004-2008 Marcel Holtmann + * Copyright (C) 2004-2005 Henryk Ploetz + * Copyright (C) 2005-2008 Brad Midgley + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#define fabs(x) ((x) < 0 ? -(x) : (x)) +/* C does not provide an explicit arithmetic shift right but this will + always be correct and every compiler *should* generate optimal code */ +#define ASR(val, bits) ((-2 >> 1 == -1) ? \ + ((int32_t)(val)) >> (bits) : ((int32_t) (val)) / (1 << (bits))) + +#define SCALE_PROTO4_TBL 15 +#define SCALE_ANA4_TBL 17 +#define SCALE_PROTO8_TBL 16 +#define SCALE_ANA8_TBL 17 +#define SCALE_SPROTO4_TBL 12 +#define SCALE_SPROTO8_TBL 14 +#define SCALE_NPROTO4_TBL 11 +#define SCALE_NPROTO8_TBL 11 +#define SCALE4_STAGE1_BITS 15 +#define SCALE4_STAGE2_BITS 16 +#define SCALE4_STAGED1_BITS 15 +#define SCALE4_STAGED2_BITS 16 +#define SCALE8_STAGE1_BITS 15 +#define SCALE8_STAGE2_BITS 15 +#define SCALE8_STAGED1_BITS 15 +#define SCALE8_STAGED2_BITS 16 + +typedef int32_t sbc_fixed_t; + +#define SCALE4_STAGE1(src) ASR(src, SCALE4_STAGE1_BITS) +#define SCALE4_STAGE2(src) ASR(src, SCALE4_STAGE2_BITS) +#define SCALE4_STAGED1(src) ASR(src, SCALE4_STAGED1_BITS) +#define SCALE4_STAGED2(src) ASR(src, SCALE4_STAGED2_BITS) +#define SCALE8_STAGE1(src) ASR(src, SCALE8_STAGE1_BITS) +#define SCALE8_STAGE2(src) ASR(src, SCALE8_STAGE2_BITS) +#define SCALE8_STAGED1(src) ASR(src, SCALE8_STAGED1_BITS) +#define SCALE8_STAGED2(src) ASR(src, SCALE8_STAGED2_BITS) + +#define SBC_FIXED_0(val) { val = 0; } +#define MUL(a, b) ((a) * (b)) +#ifdef __arm__ +#define MULA(a, b, res) ({ \ + int tmp = res; \ + __asm__( \ + "mla %0, %2, %3, %0" \ + : "=&r" (tmp) \ + : "0" (tmp), "r" (a), "r" (b)); \ + tmp; }) +#else +#define MULA(a, b, res) ((a) * (b) + (res)) +#endif diff --git a/src/modules/bluetooth/sbc_tables.h b/src/modules/bluetooth/sbc_tables.h new file mode 100644 index 00000000..7ac4e68b --- /dev/null +++ b/src/modules/bluetooth/sbc_tables.h @@ -0,0 +1,167 @@ +/* + * + * Bluetooth low-complexity, subband codec (SBC) library + * + * Copyright (C) 2004-2008 Marcel Holtmann + * Copyright (C) 2004-2005 Henryk Ploetz + * Copyright (C) 2005-2006 Brad Midgley + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +/* A2DP specification: Appendix B, page 69 */ +static const int sbc_offset4[4][4] = { + { -1, 0, 0, 0 }, + { -2, 0, 0, 1 }, + { -2, 0, 0, 1 }, + { -2, 0, 0, 1 } +}; + +/* A2DP specification: Appendix B, page 69 */ +static const int sbc_offset8[4][8] = { + { -2, 0, 0, 0, 0, 0, 0, 1 }, + { -3, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 }, + { -4, 0, 0, 0, 0, 0, 1, 2 } +}; + +#define SP4(val) ASR(val, SCALE_PROTO4_TBL) +#define SA4(val) ASR(val, SCALE_ANA4_TBL) +#define SP8(val) ASR(val, SCALE_PROTO8_TBL) +#define SA8(val) ASR(val, SCALE_ANA8_TBL) +#define SS4(val) ASR(val, SCALE_SPROTO4_TBL) +#define SS8(val) ASR(val, SCALE_SPROTO8_TBL) +#define SN4(val) ASR(val, SCALE_NPROTO4_TBL) +#define SN8(val) ASR(val, SCALE_NPROTO8_TBL) + +static const int32_t _sbc_proto_4[20] = { + SP4(0x02cb3e8c), SP4(0x22b63dc0), SP4(0x002329cc), SP4(0x053b7548), + SP4(0x31eab940), SP4(0xec1f5e60), SP4(0xff3773a8), SP4(0x0061c5a7), + SP4(0x07646680), SP4(0x3f239480), SP4(0xf89f23a8), SP4(0x007a4737), + SP4(0x00b32807), SP4(0x083ddc80), SP4(0x4825e480), SP4(0x0191e578), + SP4(0x00ff11ca), SP4(0x00fb7991), SP4(0x069fdc58), SP4(0x4b584000) +}; + +static const int32_t _anamatrix4[4] = { + SA4(0x2d413cc0), SA4(0x3b20d780), SA4(0x40000000), SA4(0x187de2a0) +}; + +static const int32_t _sbc_proto_8[40] = { + SP8(0x02e5cd20), SP8(0x22d0c200), SP8(0x006bfe27), SP8(0x07808930), + SP8(0x3f1c8800), SP8(0xf8810d70), SP8(0x002cfdc6), SP8(0x055acf28), + SP8(0x31f566c0), SP8(0xebfe57e0), SP8(0xff27c437), SP8(0x001485cc), + SP8(0x041c6e58), SP8(0x2a7cfa80), SP8(0xe4c4a240), SP8(0xfe359e4c), + SP8(0x0048b1f8), SP8(0x0686ce30), SP8(0x38eec5c0), SP8(0xf2a1b9f0), + SP8(0xffe8904a), SP8(0x0095698a), SP8(0x0824a480), SP8(0x443b3c00), + SP8(0xfd7badc8), SP8(0x00d3e2d9), SP8(0x00c183d2), SP8(0x084e1950), + SP8(0x4810d800), SP8(0x017f43fe), SP8(0x01056dd8), SP8(0x00e9cb9f), + SP8(0x07d7d090), SP8(0x4a708980), SP8(0x0488fae8), SP8(0x0113bd20), + SP8(0x0107b1a8), SP8(0x069fb3c0), SP8(0x4b3db200), SP8(0x00763f48) +}; + +static const int32_t sbc_proto_4_40m0[] = { + SS4(0x00000000), SS4(0xffa6982f), SS4(0xfba93848), SS4(0x0456c7b8), + SS4(0x005967d1), SS4(0xfffb9ac7), SS4(0xff589157), SS4(0xf9c2a8d8), + SS4(0x027c1434), SS4(0x0019118b), SS4(0xfff3c74c), SS4(0xff137330), + SS4(0xf81b8d70), SS4(0x00ec1b8b), SS4(0xfff0b71a), SS4(0xffe99b00), + SS4(0xfef84470), SS4(0xf6fb4370), SS4(0xffcdc351), SS4(0xffe01dc7) +}; + +static const int32_t sbc_proto_4_40m1[] = { + SS4(0xffe090ce), SS4(0xff2c0475), SS4(0xf694f800), SS4(0xff2c0475), + SS4(0xffe090ce), SS4(0xffe01dc7), SS4(0xffcdc351), SS4(0xf6fb4370), + SS4(0xfef84470), SS4(0xffe99b00), SS4(0xfff0b71a), SS4(0x00ec1b8b), + SS4(0xf81b8d70), SS4(0xff137330), SS4(0xfff3c74c), SS4(0x0019118b), + SS4(0x027c1434), SS4(0xf9c2a8d8), SS4(0xff589157), SS4(0xfffb9ac7) +}; + +static const int32_t sbc_proto_8_80m0[] = { + SS8(0x00000000), SS8(0xfe8d1970), SS8(0xee979f00), SS8(0x11686100), + SS8(0x0172e690), SS8(0xfff5bd1a), SS8(0xfdf1c8d4), SS8(0xeac182c0), + SS8(0x0d9daee0), SS8(0x00e530da), SS8(0xffe9811d), SS8(0xfd52986c), + SS8(0xe7054ca0), SS8(0x0a00d410), SS8(0x006c1de4), SS8(0xffdba705), + SS8(0xfcbc98e8), SS8(0xe3889d20), SS8(0x06af2308), SS8(0x000bb7db), + SS8(0xffca00ed), SS8(0xfc3fbb68), SS8(0xe071bc00), SS8(0x03bf7948), + SS8(0xffc4e05c), SS8(0xffb54b3b), SS8(0xfbedadc0), SS8(0xdde26200), + SS8(0x0142291c), SS8(0xff960e94), SS8(0xff9f3e17), SS8(0xfbd8f358), + SS8(0xdbf79400), SS8(0xff405e01), SS8(0xff7d4914), SS8(0xff8b1a31), + SS8(0xfc1417b8), SS8(0xdac7bb40), SS8(0xfdbb828c), SS8(0xff762170) +}; + +static const int32_t sbc_proto_8_80m1[] = { + SS8(0xff7c272c), SS8(0xfcb02620), SS8(0xda612700), SS8(0xfcb02620), + SS8(0xff7c272c), SS8(0xff762170), SS8(0xfdbb828c), SS8(0xdac7bb40), + SS8(0xfc1417b8), SS8(0xff8b1a31), SS8(0xff7d4914), SS8(0xff405e01), + SS8(0xdbf79400), SS8(0xfbd8f358), SS8(0xff9f3e17), SS8(0xff960e94), + SS8(0x0142291c), SS8(0xdde26200), SS8(0xfbedadc0), SS8(0xffb54b3b), + SS8(0xffc4e05c), SS8(0x03bf7948), SS8(0xe071bc00), SS8(0xfc3fbb68), + SS8(0xffca00ed), SS8(0x000bb7db), SS8(0x06af2308), SS8(0xe3889d20), + SS8(0xfcbc98e8), SS8(0xffdba705), SS8(0x006c1de4), SS8(0x0a00d410), + SS8(0xe7054ca0), SS8(0xfd52986c), SS8(0xffe9811d), SS8(0x00e530da), + SS8(0x0d9daee0), SS8(0xeac182c0), SS8(0xfdf1c8d4), SS8(0xfff5bd1a) +}; + +static const int32_t _anamatrix8[8] = { + SA8(0x3b20d780), SA8(0x187de2a0), SA8(0x3ec52f80), SA8(0x3536cc40), + SA8(0x238e7680), SA8(0x0c7c5c20), SA8(0x2d413cc0), SA8(0x40000000) +}; + +static const int32_t synmatrix4[8][4] = { + { SN4(0x05a82798), SN4(0xfa57d868), SN4(0xfa57d868), SN4(0x05a82798) }, + { SN4(0x030fbc54), SN4(0xf89be510), SN4(0x07641af0), SN4(0xfcf043ac) }, + { SN4(0x00000000), SN4(0x00000000), SN4(0x00000000), SN4(0x00000000) }, + { SN4(0xfcf043ac), SN4(0x07641af0), SN4(0xf89be510), SN4(0x030fbc54) }, + { SN4(0xfa57d868), SN4(0x05a82798), SN4(0x05a82798), SN4(0xfa57d868) }, + { SN4(0xf89be510), SN4(0xfcf043ac), SN4(0x030fbc54), SN4(0x07641af0) }, + { SN4(0xf8000000), SN4(0xf8000000), SN4(0xf8000000), SN4(0xf8000000) }, + { SN4(0xf89be510), SN4(0xfcf043ac), SN4(0x030fbc54), SN4(0x07641af0) } +}; + +static const int32_t synmatrix8[16][8] = { + { SN8(0x05a82798), SN8(0xfa57d868), SN8(0xfa57d868), SN8(0x05a82798), + SN8(0x05a82798), SN8(0xfa57d868), SN8(0xfa57d868), SN8(0x05a82798) }, + { SN8(0x0471ced0), SN8(0xf8275a10), SN8(0x018f8b84), SN8(0x06a6d988), + SN8(0xf9592678), SN8(0xfe70747c), SN8(0x07d8a5f0), SN8(0xfb8e3130) }, + { SN8(0x030fbc54), SN8(0xf89be510), SN8(0x07641af0), SN8(0xfcf043ac), + SN8(0xfcf043ac), SN8(0x07641af0), SN8(0xf89be510), SN8(0x030fbc54) }, + { SN8(0x018f8b84), SN8(0xfb8e3130), SN8(0x06a6d988), SN8(0xf8275a10), + SN8(0x07d8a5f0), SN8(0xf9592678), SN8(0x0471ced0), SN8(0xfe70747c) }, + { SN8(0x00000000), SN8(0x00000000), SN8(0x00000000), SN8(0x00000000), + SN8(0x00000000), SN8(0x00000000), SN8(0x00000000), SN8(0x00000000) }, + { SN8(0xfe70747c), SN8(0x0471ced0), SN8(0xf9592678), SN8(0x07d8a5f0), + SN8(0xf8275a10), SN8(0x06a6d988), SN8(0xfb8e3130), SN8(0x018f8b84) }, + { SN8(0xfcf043ac), SN8(0x07641af0), SN8(0xf89be510), SN8(0x030fbc54), + SN8(0x030fbc54), SN8(0xf89be510), SN8(0x07641af0), SN8(0xfcf043ac) }, + { SN8(0xfb8e3130), SN8(0x07d8a5f0), SN8(0xfe70747c), SN8(0xf9592678), + SN8(0x06a6d988), SN8(0x018f8b84), SN8(0xf8275a10), SN8(0x0471ced0) }, + { SN8(0xfa57d868), SN8(0x05a82798), SN8(0x05a82798), SN8(0xfa57d868), + SN8(0xfa57d868), SN8(0x05a82798), SN8(0x05a82798), SN8(0xfa57d868) }, + { SN8(0xf9592678), SN8(0x018f8b84), SN8(0x07d8a5f0), SN8(0x0471ced0), + SN8(0xfb8e3130), SN8(0xf8275a10), SN8(0xfe70747c), SN8(0x06a6d988) }, + { SN8(0xf89be510), SN8(0xfcf043ac), SN8(0x030fbc54), SN8(0x07641af0), + SN8(0x07641af0), SN8(0x030fbc54), SN8(0xfcf043ac), SN8(0xf89be510) }, + { SN8(0xf8275a10), SN8(0xf9592678), SN8(0xfb8e3130), SN8(0xfe70747c), + SN8(0x018f8b84), SN8(0x0471ced0), SN8(0x06a6d988), SN8(0x07d8a5f0) }, + { SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000), + SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000), SN8(0xf8000000) }, + { SN8(0xf8275a10), SN8(0xf9592678), SN8(0xfb8e3130), SN8(0xfe70747c), + SN8(0x018f8b84), SN8(0x0471ced0), SN8(0x06a6d988), SN8(0x07d8a5f0) }, + { SN8(0xf89be510), SN8(0xfcf043ac), SN8(0x030fbc54), SN8(0x07641af0), + SN8(0x07641af0), SN8(0x030fbc54), SN8(0xfcf043ac), SN8(0xf89be510) }, + { SN8(0xf9592678), SN8(0x018f8b84), SN8(0x07d8a5f0), SN8(0x0471ced0), + SN8(0xfb8e3130), SN8(0xf8275a10), SN8(0xfe70747c), SN8(0x06a6d988) } +}; -- cgit