diff options
| -rw-r--r-- | audio/a2dp.c | 611 | ||||
| -rw-r--r-- | audio/a2dp.h | 48 | ||||
| -rw-r--r-- | audio/avdtp.c | 2174 | ||||
| -rw-r--r-- | audio/avdtp.h | 177 | ||||
| -rw-r--r-- | audio/device.c | 207 | ||||
| -rw-r--r-- | audio/device.h | 7 | ||||
| -rw-r--r-- | audio/headset.c | 99 | ||||
| -rw-r--r-- | audio/headset.h | 24 | ||||
| -rw-r--r-- | audio/ipc.h | 106 | ||||
| -rw-r--r-- | audio/main.c | 48 | ||||
| -rw-r--r-- | audio/manager.c | 63 | ||||
| -rw-r--r-- | audio/manager.h | 17 | ||||
| -rw-r--r-- | audio/pcm_bluetooth.c | 541 | ||||
| -rw-r--r-- | audio/sink.c | 429 | ||||
| -rw-r--r-- | audio/sink.h | 11 | ||||
| -rw-r--r-- | audio/unix.c | 227 | ||||
| -rw-r--r-- | audio/unix.h | 5 |
17 files changed, 4451 insertions, 343 deletions
diff --git a/audio/a2dp.c b/audio/a2dp.c index db0ed7cb..3afc133d 100644 --- a/audio/a2dp.c +++ b/audio/a2dp.c @@ -25,4 +25,615 @@ #include <config.h> #endif +#include <stdlib.h> + +#include <dbus/dbus.h> +#include <glib.h> + +#include <bluetooth/sdp.h> +#include <bluetooth/sdp_lib.h> + +#include "logging.h" +#include "manager.h" +#include "avdtp.h" #include "a2dp.h" + +static DBusConnection *connection = NULL; + +static uint32_t sink_record_id = 0; +static uint32_t source_record_id = 0; + +static struct avdtp_local_sep *sink_sep = NULL; +static struct avdtp_local_sep *source_sep = NULL; + +static gboolean setconf_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, + uint8_t int_seid, GSList *caps, + uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Set_Configuration_Ind"); + else + debug("SBC Source: Set_Configuration_Ind"); + + return TRUE; +} + +static gboolean getcap_ind(struct avdtp_local_sep *sep, + GSList **caps, uint8_t *err) +{ + struct avdtp_service_capability *media_transport, *media_codec; + struct sbc_codec_cap sbc_cap; + + if (sep == sink_sep) + debug("SBC Sink: Get_Capability_Ind"); + else + debug("SBC Source: Get_Capability_Ind"); + + *caps = NULL; + + media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT, + NULL, 0); + + *caps = g_slist_append(*caps, media_transport); + + memset(&sbc_cap, 0, sizeof(struct sbc_codec_cap)); + + sbc_cap.cap.media_type = AVDTP_MEDIA_TYPE_AUDIO; + sbc_cap.cap.media_codec_type = A2DP_CODEC_SBC; + + sbc_cap.frequency = ( A2DP_SAMPLING_FREQ_48000 | + A2DP_SAMPLING_FREQ_44100 | + A2DP_SAMPLING_FREQ_32000 | + A2DP_SAMPLING_FREQ_16000 ); + + sbc_cap.channel_mode = ( A2DP_CHANNEL_MODE_JOINT_STEREO | + A2DP_CHANNEL_MODE_STEREO | + A2DP_CHANNEL_MODE_DUAL_CHANNEL | + A2DP_CHANNEL_MODE_MONO ); + + sbc_cap.block_length = ( A2DP_BLOCK_LENGTH_16 | + A2DP_BLOCK_LENGTH_12 | + A2DP_BLOCK_LENGTH_8 | + A2DP_BLOCK_LENGTH_4 ); + + sbc_cap.subbands = ( A2DP_SUBBANDS_8 | A2DP_SUBBANDS_4 ); + + sbc_cap.allocation_method = ( A2DP_ALLOCATION_LOUDNESS | + A2DP_ALLOCATION_SNR ); + + sbc_cap.min_bitpool = 2; + sbc_cap.max_bitpool = 250; + + media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, &sbc_cap, + sizeof(sbc_cap)); + + *caps = g_slist_append(*caps, media_codec); + + return TRUE; +} + +static void setconf_cfm(struct avdtp_local_sep *sep, + struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Set_Configuration_Cfm"); + else + debug("SBC Source: Set_Configuration_Cfm"); +} + +static gboolean getconf_ind(struct avdtp_local_sep *sep, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Get_Configuration_Ind"); + else + debug("SBC Source: Get_Configuration_Ind"); + return TRUE; +} + +static void getconf_cfm(struct avdtp_local_sep *sep, + struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Set_Configuration_Cfm"); + else + debug("SBC Source: Set_Configuration_Cfm"); +} + +static gboolean open_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Open_Ind"); + else + debug("SBC Source: Open_Ind"); + return TRUE; +} + +static void open_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Open_Cfm"); + else + debug("SBC Source: Open_Cfm"); +} + +static gboolean start_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Start_Ind"); + else + debug("SBC Source: Start_Ind"); + return TRUE; +} + +static void start_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Start_Cfm"); + else + debug("SBC Source: Start_Cfm"); +} + +static gboolean suspend_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Suspend_Ind"); + else + debug("SBC Source: Suspend_Ind"); + return TRUE; +} + +static void suspend_cfm(struct avdtp_local_sep *sep, + struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Suspend_Cfm"); + else + debug("SBC Source: Suspend_Cfm"); +} + +static gboolean close_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Close_Ind"); + else + debug("SBC Source: Close_Ind"); + return TRUE; +} + +static void close_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Close_Cfm"); + else + debug("SBC Source: Close_Cfm"); +} + +static gboolean abort_ind(struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: Abort_Ind"); + else + debug("SBC Source: Abort_Ind"); + return TRUE; +} + +static void abort_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream) +{ + if (sep == sink_sep) + debug("SBC Sink: Abort_Cfm"); + else + debug("SBC Source: Abort_Cfm"); +} + +static gboolean reconf_ind(struct avdtp_local_sep *sep, uint8_t *err) +{ + if (sep == sink_sep) + debug("SBC Sink: ReConfigure_Ind"); + else + debug("SBC Source: ReConfigure_Ind"); + return TRUE; +} + +static void reconf_cfm(struct avdtp_local_sep *sep) +{ + if (sep == sink_sep) + debug("SBC Sink: ReConfigure_Cfm"); + else + debug("SBC Source: ReConfigure_Cfm"); +} + +static struct avdtp_sep_cfm cfm = { + .set_configuration = setconf_cfm, + .get_configuration = getconf_cfm, + .open = open_cfm, + .start = start_cfm, + .suspend = suspend_cfm, + .close = close_cfm, + .abort = abort_cfm, + .reconfigure = reconf_cfm +}; + +static struct avdtp_sep_ind ind = { + .get_capability = getcap_ind, + .set_configuration = setconf_ind, + .get_configuration = getconf_ind, + .open = open_ind, + .start = start_ind, + .suspend = suspend_ind, + .close = close_ind, + .abort = abort_ind, + .reconfigure = reconf_ind +}; + +static int a2dp_source_record(sdp_buf_t *buf) +{ + sdp_list_t *svclass_id, *pfseq, *apseq, *root; + uuid_t root_uuid, l2cap, avdtp, a2src; + sdp_profile_desc_t profile[1]; + sdp_list_t *aproto, *proto[2]; + sdp_record_t record; + sdp_data_t *psm, *version, *features; + uint16_t lp = AVDTP_UUID, ver = 0x0100, feat = 0x000F; + int ret = 0; + + memset(&record, 0, sizeof(sdp_record_t)); + + sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); + root = sdp_list_append(0, &root_uuid); + sdp_set_browse_groups(&record, root); + + sdp_uuid16_create(&a2src, AUDIO_SOURCE_SVCLASS_ID); + svclass_id = sdp_list_append(0, &a2src); + sdp_set_service_classes(&record, svclass_id); + + sdp_uuid16_create(&profile[0].uuid, ADVANCED_AUDIO_PROFILE_ID); + profile[0].version = 0x0100; + pfseq = sdp_list_append(0, &profile[0]); + sdp_set_profile_descs(&record, pfseq); + + sdp_uuid16_create(&l2cap, L2CAP_UUID); + proto[0] = sdp_list_append(0, &l2cap); + psm = sdp_data_alloc(SDP_UINT16, &lp); + proto[0] = sdp_list_append(proto[0], psm); + apseq = sdp_list_append(0, proto[0]); + + sdp_uuid16_create(&avdtp, AVDTP_UUID); + proto[1] = sdp_list_append(0, &avdtp); + version = sdp_data_alloc(SDP_UINT16, &ver); + proto[1] = sdp_list_append(proto[1], version); + apseq = sdp_list_append(apseq, proto[1]); + + aproto = sdp_list_append(0, apseq); + sdp_set_access_protos(&record, aproto); + + features = sdp_data_alloc(SDP_UINT16, &feat); + sdp_attr_add(&record, SDP_ATTR_SUPPORTED_FEATURES, features); + + sdp_set_info_attr(&record, "Audio Source", 0, 0); + + if (sdp_gen_record_pdu(&record, buf) < 0) + ret = -1; + else + ret = 0; + + free(psm); + free(version); + sdp_list_free(proto[0], 0); + sdp_list_free(proto[1], 0); + sdp_list_free(apseq, 0); + sdp_list_free(pfseq, 0); + sdp_list_free(aproto, 0); + sdp_list_free(root, 0); + sdp_list_free(svclass_id, 0); + sdp_list_free(record.attrlist, (sdp_free_func_t) sdp_data_free); + sdp_list_free(record.pattern, free); + + return ret; +} + +static int a2dp_sink_record(sdp_buf_t *buf) +{ + return 0; +} + +int a2dp_init(DBusConnection *conn, gboolean enable_sink, gboolean enable_source) +{ + sdp_buf_t buf; + + if (!enable_sink && !enable_source) + return 0; + + connection = dbus_connection_ref(conn); + + avdtp_init(); + + if (enable_sink) { + source_sep = avdtp_register_sep(AVDTP_SEP_TYPE_SOURCE, + AVDTP_MEDIA_TYPE_AUDIO, + &ind, &cfm); + if (source_sep == NULL) + return -1; + + if (a2dp_source_record(&buf) < 0) { + error("Unable to allocate new service record"); + return -1; + } + + source_record_id = add_service_record(conn, &buf); + free(buf.data); + if (!source_record_id) { + error("Unable to register A2DP Source service record"); + return -1; + } + } + + if (enable_source) { + sink_sep = avdtp_register_sep(AVDTP_SEP_TYPE_SINK, + AVDTP_MEDIA_TYPE_AUDIO, + &ind, &cfm); + if (sink_sep == NULL) + return -1; + + if (a2dp_sink_record(&buf) < 0) { + error("Unable to allocate new service record"); + return -1; + } + + sink_record_id = add_service_record(conn, &buf); + free(buf.data); + if (!sink_record_id) { + error("Unable to register A2DP Sink service record"); + return -1; + } + } + + return 0; +} + +void a2dp_exit() +{ + if (sink_sep) + avdtp_unregister_sep(sink_sep); + + if (source_sep) + avdtp_unregister_sep(source_sep); + + if (source_record_id) { + remove_service_record(connection, source_record_id); + source_record_id = 0; + } + + if (sink_record_id) { + remove_service_record(connection, sink_record_id); + sink_record_id = 0; + } + + dbus_connection_unref(connection); +} + +static uint8_t default_bitpool(uint8_t freq, uint8_t mode) { + switch (freq) { + case A2DP_SAMPLING_FREQ_16000: + case A2DP_SAMPLING_FREQ_32000: + return 53; + case A2DP_SAMPLING_FREQ_44100: + switch (mode) { + case A2DP_CHANNEL_MODE_MONO: + case A2DP_CHANNEL_MODE_DUAL_CHANNEL: + return 31; + case A2DP_CHANNEL_MODE_STEREO: + case A2DP_CHANNEL_MODE_JOINT_STEREO: + return 53; + default: + error("Invalid channel mode %u", mode); + return 53; + } + case A2DP_SAMPLING_FREQ_48000: + switch (mode) { + case A2DP_CHANNEL_MODE_MONO: + case A2DP_CHANNEL_MODE_DUAL_CHANNEL: + return 29; + case A2DP_CHANNEL_MODE_STEREO: + case A2DP_CHANNEL_MODE_JOINT_STEREO: + return 51; + default: + error("Invalid channel mode %u", mode); + return 51; + } + default: + error("Invalid sampling freq %u", freq); + return 53; + } +} + +static gboolean select_sbc_params(struct sbc_codec_cap *cap, + struct sbc_codec_cap *supported) +{ + uint max_bitpool, min_bitpool; + + memset(cap, 0, sizeof(struct sbc_codec_cap)); + + cap->cap.media_type = AVDTP_MEDIA_TYPE_AUDIO; + cap->cap.media_codec_type = A2DP_CODEC_SBC; + + if (supported->frequency & A2DP_SAMPLING_FREQ_48000) + cap->frequency = A2DP_SAMPLING_FREQ_48000; + else if (supported->frequency & A2DP_SAMPLING_FREQ_44100) + cap->frequency = A2DP_SAMPLING_FREQ_44100; + else if (supported->frequency & A2DP_SAMPLING_FREQ_32000) + cap->frequency = A2DP_SAMPLING_FREQ_32000; + else if (supported->frequency & A2DP_SAMPLING_FREQ_16000) + cap->frequency = A2DP_SAMPLING_FREQ_16000; + else { + error("No supported frequencies"); + return FALSE; + } + + if (supported->channel_mode & A2DP_CHANNEL_MODE_JOINT_STEREO) + cap->channel_mode = A2DP_CHANNEL_MODE_JOINT_STEREO; + else if (supported->channel_mode & A2DP_CHANNEL_MODE_STEREO) + cap->channel_mode = A2DP_CHANNEL_MODE_STEREO; + else if (supported->channel_mode & A2DP_CHANNEL_MODE_DUAL_CHANNEL) + cap->channel_mode = A2DP_CHANNEL_MODE_DUAL_CHANNEL; + else if (supported->channel_mode & A2DP_CHANNEL_MODE_MONO) + cap->channel_mode = A2DP_CHANNEL_MODE_MONO; + else { + error("No supported channel modes"); + return FALSE; + } + + if (supported->block_length & A2DP_BLOCK_LENGTH_16) + cap->block_length = A2DP_BLOCK_LENGTH_16; + else if (supported->block_length & A2DP_BLOCK_LENGTH_12) + cap->block_length = A2DP_BLOCK_LENGTH_12; + else if (supported->block_length & A2DP_BLOCK_LENGTH_8) + cap->block_length = A2DP_BLOCK_LENGTH_8; + else if (supported->block_length & A2DP_BLOCK_LENGTH_4) + cap->block_length = A2DP_BLOCK_LENGTH_4; + else { + error("No supported block lengths"); + return FALSE; + } + + if (supported->subbands & A2DP_SUBBANDS_8) + cap->subbands = A2DP_SUBBANDS_8; + else if (supported->subbands & A2DP_SUBBANDS_4) + cap->subbands = A2DP_SUBBANDS_4; + else { + error("No supported subbands"); + return FALSE; + } + + if (supported->allocation_method & A2DP_ALLOCATION_LOUDNESS) + cap->allocation_method = A2DP_ALLOCATION_LOUDNESS; + else if (supported->allocation_method & A2DP_ALLOCATION_SNR) + cap->allocation_method = A2DP_ALLOCATION_SNR; + + min_bitpool = MIN(default_bitpool(cap->frequency, cap->channel_mode), + supported->min_bitpool); + max_bitpool = MIN(default_bitpool(cap->frequency, cap->channel_mode), + supported->max_bitpool); + + cap->min_bitpool = min_bitpool; + cap->max_bitpool = max_bitpool; + + return TRUE; +} + +gboolean a2dp_select_capabilities(struct avdtp_remote_sep *rsep, GSList **caps) +{ + struct avdtp_service_capability *media_transport, *media_codec; + struct sbc_codec_cap sbc_cap, *acp_sbc; + + media_codec = avdtp_get_codec(rsep); + if (!media_codec) + return FALSE; + + acp_sbc = (void *) media_codec->data; + + media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT, + NULL, 0); + + *caps = g_slist_append(*caps, media_transport); + + select_sbc_params(&sbc_cap, acp_sbc); + + media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, &sbc_cap, + sizeof(sbc_cap)); + + *caps = g_slist_append(*caps, media_codec); + + return TRUE; +} + +gboolean a2dp_get_config(struct avdtp_stream *stream, struct ipc_data_cfg **cfg) +{ + struct avdtp_service_capability *cap; + struct avdtp_media_codec_capability *codec_cap = NULL; + struct sbc_codec_cap *sbc_cap; + struct ipc_data_cfg *rsp; + struct ipc_codec_sbc *sbc; + GSList *caps; + + rsp = g_malloc0(sizeof(struct ipc_data_cfg) + + sizeof(struct ipc_codec_sbc)); + rsp->fd = -1; + sbc = (void *) rsp->data; + + if (!avdtp_stream_get_transport(stream, &rsp->fd, &rsp->pkt_len, + &caps)) { + g_free(rsp); + return FALSE; + } + + for (; caps; caps = g_slist_next(caps)) { + cap = caps->data; + if (cap->category == AVDTP_MEDIA_CODEC) { + codec_cap = (void *) cap->data; + break; + } + } + + if (codec_cap == NULL) { + g_free(rsp); + return FALSE; + } + + rsp->fd_opt = CFG_FD_OPT_WRITE; + + *cfg = rsp; + + if (codec_cap->media_codec_type != A2DP_CODEC_SBC) + return TRUE; + + sbc_cap = (struct sbc_codec_cap *) codec_cap; + rsp->channels = sbc_cap->channel_mode == + A2DP_CHANNEL_MODE_MONO ? 1 : 2; + rsp->channel_mode = sbc_cap->channel_mode; + rsp->sample_size = 2; + + switch (sbc_cap->frequency) { + case A2DP_SAMPLING_FREQ_16000: + rsp->rate = 16000; + break; + case A2DP_SAMPLING_FREQ_32000: + rsp->rate = 32000; + break; + case A2DP_SAMPLING_FREQ_44100: + rsp->rate = 44100; + break; + case A2DP_SAMPLING_FREQ_48000: + rsp->rate = 48000; + break; + } + + rsp->codec = CFG_CODEC_SBC; + sbc->allocation = sbc_cap->allocation_method == A2DP_ALLOCATION_SNR ? + 0x01 : 0x00; + sbc->subbands = sbc_cap->subbands == A2DP_SUBBANDS_4 ? 4 : 8; + + switch (sbc_cap->block_length) { + case A2DP_BLOCK_LENGTH_4: + sbc->blocks = 4; + break; + case A2DP_BLOCK_LENGTH_8: + sbc->blocks = 8; + break; + case A2DP_BLOCK_LENGTH_12: + sbc->blocks = 12; + break; + case A2DP_BLOCK_LENGTH_16: + sbc->blocks = 16; + break; + } + + sbc->bitpool = sbc_cap->max_bitpool; + + return TRUE; +} diff --git a/audio/a2dp.h b/audio/a2dp.h index 153a1731..7e53f7a9 100644 --- a/audio/a2dp.h +++ b/audio/a2dp.h @@ -21,3 +21,51 @@ * */ +#include <dbus.h> +#include <glib.h> +#include "avdtp.h" + +#define A2DP_CODEC_SBC 0x00 +#define A2DP_CODEC_MPEG12 0x01 +#define A2DP_CODEC_MPEG24 0x02 +#define A2DP_CODEC_ATRAC 0x03 + +#define A2DP_SAMPLING_FREQ_16000 (1 << 3) +#define A2DP_SAMPLING_FREQ_32000 (1 << 2) +#define A2DP_SAMPLING_FREQ_44100 (1 << 1) +#define A2DP_SAMPLING_FREQ_48000 1 + +#define A2DP_CHANNEL_MODE_MONO (1 << 3) +#define A2DP_CHANNEL_MODE_DUAL_CHANNEL (1 << 2) +#define A2DP_CHANNEL_MODE_STEREO (1 << 1) +#define A2DP_CHANNEL_MODE_JOINT_STEREO 1 + +#define A2DP_BLOCK_LENGTH_4 (1 << 3) +#define A2DP_BLOCK_LENGTH_8 (1 << 2) +#define A2DP_BLOCK_LENGTH_12 (1 << 1) +#define A2DP_BLOCK_LENGTH_16 1 + +#define A2DP_SUBBANDS_4 (1 << 1) +#define A2DP_SUBBANDS_8 1 + +#define A2DP_ALLOCATION_SNR (1 << 1) +#define A2DP_ALLOCATION_LOUDNESS 1 + +struct sbc_codec_cap { + struct avdtp_media_codec_capability cap; + uint8_t channel_mode:4; + uint8_t frequency:4; + uint8_t allocation_method:2; + uint8_t subbands:2; + uint8_t block_length:4; + uint8_t min_bitpool; + uint8_t max_bitpool; +} __attribute__ ((packed)); + +int a2dp_init(DBusConnection *conn, gboolean enable_sink, + gboolean enable_source); +void a2dp_exit(void); + +gboolean a2dp_select_capabilities(struct avdtp_remote_sep *rsep, GSList **caps); + +gboolean a2dp_get_config(struct avdtp_stream *stream, struct ipc_data_cfg **cfg); diff --git a/audio/avdtp.c b/audio/avdtp.c index 2f4ab59e..8615a18a 100644 --- a/audio/avdtp.c +++ b/audio/avdtp.c @@ -24,3 +24,2177 @@ #ifdef HAVE_CONFIG_H #include <config.h> #endif + +#include <stdio.h> +#include <errno.h> +#include <unistd.h> +#include <assert.h> +#include <signal.h> +#include <netinet/in.h> + +#include <glib.h> + +#include "avdtp.h" +#include "logging.h" +#include "dbus.h" + +#include <bluetooth/l2cap.h> + +#define AVDTP_PSM 25 + +#define MAX_SEID 0x3E + +#define AVDTP_DISCOVER 0x01 +#define AVDTP_GET_CAPABILITIES 0x02 +#define AVDTP_SET_CONFIGURATION 0x03 +#define AVDTP_GET_CONFIGURATION 0x04 +#define AVDTP_RECONFIGURE 0x05 +#define AVDTP_OPEN 0x06 +#define AVDTP_START 0x07 +#define AVDTP_CLOSE 0x08 +#define AVDTP_SUSPEND 0x09 +#define AVDTP_ABORT 0x0A +#define AVDTP_SECURITY_CONTROL 0x0B + +#define AVDTP_PKT_TYPE_SINGLE 0x00 +#define AVDTP_PKT_TYPE_START 0x01 +#define AVDTP_PKT_TYPE_CONTINUE 0x02 +#define AVDTP_PKT_TYPE_END 0x03 + +#define AVDTP_MSG_TYPE_COMMAND 0x00 +#define AVDTP_MSG_TYPE_ACCEPT 0x02 +#define AVDTP_MSG_TYPE_REJECT 0x03 + +#define REQ_TIMEOUT 2000 +#define DISCONNECT_TIMEOUT 5000 + +typedef enum { + AVDTP_ERROR_ERRNO, + AVDTP_ERROR_ERROR_CODE +} avdtp_error_type_t; + +typedef enum { + AVDTP_SESSION_STATE_DISCONNECTED, + AVDTP_SESSION_STATE_CONNECTING, + AVDTP_SESSION_STATE_CONNECTED +} avdtp_session_state_t; + +struct avdtp_header { + uint8_t message_type:2; + uint8_t packet_type:2; + uint8_t transaction:4; + uint8_t signal_id:6; + uint8_t rfa0:2; +} __attribute__ ((packed)); + +typedef struct seid_info { + uint8_t rfa0:1; + uint8_t inuse:1; + uint8_t seid:6; + uint8_t rfa2:3; + uint8_t type:1; + uint8_t media_type:4; +} __attribute__ ((packed)) seid_info_t; + +/* packets */ + +struct discover_req { + struct avdtp_header header; +} __attribute__ ((packed)); + +struct discover_resp { + struct avdtp_header header; + struct seid_info seps[0]; +} __attribute__ ((packed)); + +struct getcap_resp { + struct avdtp_header header; + uint8_t caps[0]; +} __attribute__ ((packed)); + +struct seid_req { + struct avdtp_header header; + uint8_t rfa0:2; + uint8_t acp_seid:6; +} __attribute__ ((packed)); + +struct seid_rej { + struct avdtp_header header; + uint8_t error; +} __attribute__ ((packed)); + +struct setconf_req { + struct avdtp_header header; + + uint8_t rfa0:2; + uint8_t acp_seid:6; + uint8_t rfa1:2; + uint8_t int_seid:6; + + uint8_t caps[0]; +} __attribute__ ((packed)); + +struct setconf_resp { + struct avdtp_header header; +} __attribute__ ((packed)); + +struct conf_rej { + struct avdtp_header header; + uint8_t category; + uint8_t error; +} __attribute__ ((packed)); + +struct stream_rej { + struct avdtp_header header; + uint8_t rfa0; + uint8_t acp_seid:6; + uint8_t error_code; +} __attribute__ ((packed)); + +struct reconf_req { + struct avdtp_header header; + + uint8_t rfa0:2; + uint8_t acp_seid:6; + + uint8_t serv_cap; + uint8_t serv_cap_len; + + uint8_t caps[0]; +} __attribute__ ((packed)); + +struct reconf_resp { + struct avdtp_header header; +} __attribute__ ((packed)); + +struct abort_resp { + struct avdtp_header header; +} __attribute__ ((packed)); + +struct stream_pause_resp { + struct avdtp_header header; + uint8_t rfa0:2; + uint8_t acp_seid:6; + uint8_t error; +} __attribute__ ((packed)); + +struct avdtp_general_rej { + uint8_t message_type:2; + uint8_t packet_type:2; + uint8_t transaction:4; + uint8_t rfa0; +} __attribute__ ((packed)); + +struct pending_req { + struct avdtp_header *msg; + int msg_size; + struct avdtp_stream *stream; /* Set if the request targeted a stream */ + guint timeout; +}; + +struct avdtp_remote_sep { + uint8_t seid; + uint8_t type; + uint8_t media_type; + struct avdtp_service_capability *codec; + GSList *caps; /* of type struct avdtp_service_capability */ + struct avdtp_stream *stream; +}; + +struct avdtp_local_sep { + avdtp_state_t state; + struct avdtp_stream *stream; + struct seid_info info; + uint8_t codec; + GSList *caps; + struct avdtp_sep_ind *ind; + struct avdtp_sep_cfm *cfm; + void *data; +}; + +struct avdtp_stream { + int sock; + uint16_t mtu; + struct avdtp *session; + struct avdtp_local_sep *lsep; + struct avdtp_remote_sep *rsep; + GSList *caps; + avdtp_stream_state_cb cb; + void *user_data; + guint io; /* Transport GSource ID */ + guint close_timer; /* Waiting for other side to close transport */ + gboolean open_acp; /* If we are in ACT role for Open */ + gboolean close_int; /* If we are in INT role for Close */ +}; + +/* Structure describing an AVDTP connection between two devices */ +struct avdtp { + int ref; + + bdaddr_t src; + bdaddr_t dst; + + avdtp_session_state_t last_state; + avdtp_session_state_t state; + + guint io; + int sock; + + GSList *seps; /* Elements of type struct avdtp_remote_sep * */ + + GSList *streams; /* Elements of type struct avdtp_stream * */ + + GSList *req_queue; /* Elements of type struct pending_req * */ + GSList *prio_queue; /* Same as req_queue but is processed before it */ + + struct avdtp_stream *pending_open; + + uint16_t mtu; + char *buf; + + avdtp_discover_cb_t discov_cb; + void *user_data; + + struct pending_req *req; + + guint dc_timer; +}; + +struct avdtp_error { + avdtp_error_type_t type; + union { + uint8_t error_code; + int posix_errno; + } err; +}; + +static uint8_t free_seid = 1; +static GSList *local_seps = NULL; + +static GIOChannel *avdtp_server = NULL; + +static GSList *sessions = NULL; + +static int send_request(struct avdtp *session, gboolean priority, + struct avdtp_stream *stream, void *buffer, int size); +static gboolean avdtp_parse_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct avdtp_header *header, int size); +static gboolean avdtp_parse_rej(struct avdtp *session, + struct avdtp_stream *stream, + struct avdtp_header *header, int size); +static int process_queue(struct avdtp *session); + +static const char *avdtp_statestr(avdtp_state_t state) +{ + switch (state) { + case AVDTP_STATE_IDLE: + return "IDLE"; + case AVDTP_STATE_CONFIGURED: + return "CONFIGURED"; + case AVDTP_STATE_OPEN: + return "OPEN"; + case AVDTP_STATE_STREAMING: + return "STREAMING"; + case AVDTP_STATE_CLOSING: + return "CLOSING"; + case AVDTP_STATE_ABORTING: + return "ABORTING"; + default: + return "<unknown state>"; + } +} + +static gboolean avdtp_send(struct avdtp *session, void *data, int len) +{ + int ret; + + ret = send(session->sock, data, len, 0); + + if (ret < 0) + ret = -errno; + else if (ret != len) + ret = -EIO; + + if (ret < 0) { + error("avdtp_send: %s (%d)", strerror(-ret), -ret); + return FALSE; + } + + return TRUE; +} + +static void pending_req_free(struct pending_req *req) +{ + if (req->timeout) + g_source_remove(req->timeout); + g_free(req->msg); + g_free(req); +} + +#if 0 +static gboolean stream_close_timeout(gpointer user_data) +{ + struct avdtp_stream *stream = user_data; + + close(stream->sock); + + return FALSE; +} +#endif + +static gboolean disconnect_timeout(gpointer user_data) +{ + struct avdtp *session = user_data; + + assert(session->ref == 1); + + sessions = g_slist_remove(sessions, session); + + session->dc_timer = 0; + + avdtp_unref(session); + + return FALSE; +} + +static void remove_disconnect_timer(struct avdtp *session) +{ + g_source_remove(session->dc_timer); + session->dc_timer = 0; +} + +static void set_disconnect_timer(struct avdtp *session) +{ + if (session->dc_timer) + remove_disconnect_timer(session); + + session->dc_timer = g_timeout_add(DISCONNECT_TIMEOUT, + disconnect_timeout, session); +} + +static void avdtp_error_init(struct avdtp_error *err, uint8_t type, int id) +{ + err->type = type; + switch (type) { + case AVDTP_ERROR_ERRNO: + err->err.posix_errno = id; + break; + case AVDTP_ERROR_ERROR_CODE: + err->err.error_code = id; + break; + } +} + +static void stream_free(struct avdtp_stream *stream) +{ + stream->lsep->info.inuse = 0; + stream->lsep->stream = NULL; + stream->rsep->stream = NULL; + if (stream->caps) { + g_slist_foreach(stream->caps, (GFunc) g_free, NULL); + g_slist_free(stream->caps); + } + g_free(stream); +} + +static void avdtp_sep_set_state(struct avdtp *session, + struct avdtp_local_sep *sep, + avdtp_state_t state) +{ + struct avdtp_stream *stream = sep->stream; + avdtp_state_t old_state; + + if (sep->state == state) + return; + + debug("stream state changed: %s -> %s", avdtp_statestr(sep->state), + avdtp_statestr(state)); + + old_state = sep->state; + sep->state = state; + + if (stream && stream->cb) + stream->cb(stream, old_state, state, NULL, + stream->user_data); + + if (state == AVDTP_STATE_IDLE) { + session->streams = g_slist_remove(session->streams, stream); + stream_free(stream); + } +} + +static void finalize_discovery(struct avdtp *session, int err) +{ + if (!session->discov_cb) + return; + + session->discov_cb(session, session->seps, err, + session->user_data); + + session->discov_cb = NULL; + session->user_data = NULL; +} + +static void release_stream(struct avdtp_stream *stream, struct avdtp *session) +{ + if (stream->sock >= 0) + close(stream->sock); + if (stream->io) + g_source_remove(stream->io); + avdtp_sep_set_state(session, stream->lsep, AVDTP_STATE_IDLE); +} + +static void connection_lost(struct avdtp *session) +{ + if (session->state == AVDTP_SESSION_STATE_CONNECTED) { + char address[18]; + + ba2str(&session->dst, address); + debug("Disconnected from %s", address); + } + + if (session->discov_cb) + finalize_discovery(session, -ECONNABORTED); + + if (session->sock >= 0) { + close(session->sock); + session->sock = -1; + } + + session->state = AVDTP_SESSION_STATE_DISCONNECTED; + + if (session->io) { + g_source_remove(session->io); + session->io = 0; + } + + g_slist_foreach(session->streams, (GFunc) release_stream, session); + session->streams = NULL; +} + +void avdtp_unref(struct avdtp *session) +{ + if (!session) + return; + + if (!g_slist_find(sessions, session)) { + error("avdtp_unref: trying to unref a unknown session"); + return; + } + + session->ref--; + + debug("avdtp_unref: ref=%d", session->ref); + + if (session->ref == 1) { + if (session->dc_timer) + remove_disconnect_timer(session); + if (session->sock >= 0) + set_disconnect_timer(session); + return; + } + + if (session->ref > 0) + return; + + if (session->dc_timer) + remove_disconnect_timer(session); + + connection_lost(session); + + sessions = g_slist_remove(sessions, session); + + if (session->req) + pending_req_free(session->req); + + g_slist_foreach(session->seps, (GFunc) g_free, NULL); + g_slist_free(session->seps); + + g_free(session->buf); + + g_free(session); +} + +struct avdtp *avdtp_ref(struct avdtp *session) +{ + session->ref++; + debug("avdtp_ref: ref=%d", session->ref); + if (session->dc_timer) + remove_disconnect_timer(session); + return session; +} + +static struct avdtp_local_sep *find_local_sep_by_seid(uint8_t seid) +{ + GSList *l; + + for (l = local_seps; l != NULL; l = g_slist_next(l)) { + struct avdtp_local_sep *sep = l->data; + + if (sep->info.seid == seid) + return sep; + } + + return NULL; +} + +static struct avdtp_local_sep *find_local_sep(uint8_t type, uint8_t media_type, + uint8_t codec) +{ + GSList *l; + + for (l = local_seps; l != NULL; l = g_slist_next(l)) { + struct avdtp_local_sep *sep = l->data; + + if (sep->info.inuse) + continue; + + if (sep->info.type == type && + sep->info.media_type == media_type && + sep->codec == codec) + return sep; + } + + return NULL; +} + +static void init_response(struct avdtp_header *rsp, struct avdtp_header *req, + gboolean accept) +{ + rsp->packet_type = AVDTP_PKT_TYPE_SINGLE; + rsp->message_type = accept ? AVDTP_MSG_TYPE_ACCEPT : + AVDTP_MSG_TYPE_REJECT; + rsp->transaction = req->transaction; + rsp->signal_id = req->signal_id; + rsp->rfa0 = 0; +} + +static gboolean avdtp_unknown_cmd(struct avdtp *session, + struct avdtp_header *req, int size) +{ + struct avdtp_general_rej rej; + + memset(&rej, 0, sizeof(rej)); + + rej.packet_type = AVDTP_PKT_TYPE_SINGLE; + rej.message_type = AVDTP_MSG_TYPE_REJECT; + rej.transaction = req->transaction; + + return avdtp_send(session, &rej, sizeof(rej)); +} + +static gboolean avdtp_discover_cmd(struct avdtp *session, + struct discover_req *req, int size) +{ + GSList *l; + struct discover_resp *rsp = (struct discover_resp *) session->buf; + struct seid_info *info; + int rsp_size; + + init_response(&rsp->header, &req->header, TRUE); + rsp_size = sizeof(struct discover_resp); + info = rsp->seps; + + for (l = local_seps; l != NULL; l = l->next) { + struct avdtp_local_sep *sep = l->data; + + if (rsp_size + sizeof(struct seid_info) > session->mtu) + break; + + memcpy(&info, &sep->info, sizeof(struct seid_info)); + rsp_size += sizeof(struct seid_info); + info++; + } + + return avdtp_send(session, session->buf, rsp_size); +} + +static gboolean avdtp_getcap_cmd(struct avdtp *session, + struct seid_req *req, int size) +{ + GSList *l, *caps; + struct avdtp_local_sep *sep = NULL; + struct seid_rej rej; + struct getcap_resp *rsp = (struct getcap_resp *) session->buf; + int rsp_size; + unsigned char *ptr; + uint8_t err; + + if (size < sizeof(struct seid_req)) { + error("Too short getcap request"); + return FALSE; + } + + sep = find_local_sep_by_seid(req->acp_seid); + if (!sep) { + err = AVDTP_BAD_ACP_SEID; + goto failed; + } + + if (!sep->ind->get_capability(sep, &caps, &err)) + goto failed; + + init_response(&rsp->header, &req->header, TRUE); + rsp_size = sizeof(struct getcap_resp); + ptr = rsp->caps; + + for (l = caps; l != NULL; l = g_slist_next(l)) { + struct avdtp_service_capability *cap = l->data; + + if (rsp_size + cap->length + 2 > session->mtu) + break; + + memcpy(ptr, cap, cap->length + 2); + rsp_size += cap->length + 2; + ptr += cap->length + 2; + + g_free(cap); + } + + g_slist_free(caps); + + return avdtp_send(session, session->buf, rsp_size); + +failed: + init_response(&rej.header, &req->header, FALSE); + rej.error = AVDTP_BAD_ACP_SEID; + return avdtp_send(session, &rej, sizeof(rej)); +} + +static gboolean avdtp_setconf_cmd(struct avdtp *session, + struct setconf_req *req, int size) +{ + struct conf_rej rej; + struct setconf_resp *rsp = (struct setconf_resp *) session->buf; + struct avdtp_local_sep *lsep; + gboolean ret; + + if (size < sizeof(struct setconf_req)) { + error("Too short getcap request"); + return FALSE; + } + + lsep = find_local_sep_by_seid(req->acp_seid); + if (!lsep || !lsep->stream) { + init_response(&rej.header, &req->header, FALSE); + rej.error = AVDTP_BAD_ACP_SEID; + return avdtp_send(session, &rej, sizeof(rej)); + } + + init_response(&rsp->header, &req->header, TRUE); + + ret = avdtp_send(session, rsp, sizeof(struct setconf_req)); + + if (ret) + avdtp_sep_set_state(session, lsep, AVDTP_STATE_CONFIGURED); + + return ret; +} + +static gboolean avdtp_getconf_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_reconf_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_open_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_start_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_close_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_suspend_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_abort_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + struct avdtp_local_sep *sep; + struct abort_resp *rsp = (struct abort_resp *) session->buf; + struct seid_rej rej; + uint8_t err; + gboolean ret; + + if (size < sizeof(struct seid_req)) { + error("Too short abort request"); + return FALSE; + } + + sep = find_local_sep_by_seid(req->acp_seid); + if (!sep || !sep->stream) { + err = AVDTP_BAD_ACP_SEID; + goto failed; + } + + if (sep->ind && sep->ind->abort) { + if (!sep->ind->abort(sep, sep->stream, &err)) + goto failed; + } + + init_response(&rsp->header, &req->header, TRUE); + ret = avdtp_send(session, rsp, sizeof(struct abort_resp)); + + if (ret) + avdtp_sep_set_state(session, sep, AVDTP_STATE_ABORTING); + + return ret; + +failed: + init_response(&rej.header, &req->header, FALSE); + rej.error = err; + return avdtp_send(session, &rej, sizeof(rej)); +} + +static gboolean avdtp_secctl_cmd(struct avdtp *session, struct seid_req *req, + int size) +{ + return avdtp_unknown_cmd(session, (void *) req, size); +} + +static gboolean avdtp_parse_cmd(struct avdtp *session, + struct avdtp_header *header, int size) +{ + switch (header->signal_id) { + case AVDTP_DISCOVER: + debug("Received DISCOVER_CMD"); + return avdtp_discover_cmd(session, (void *) header, size); + case AVDTP_GET_CAPABILITIES: + debug("Received GET_CAPABILITIES_CMD"); + return avdtp_getcap_cmd(session, (void *) header, size); + case AVDTP_SET_CONFIGURATION: + debug("Received SET_CONFIGURATION_CMD"); + return avdtp_setconf_cmd(session, (void *) header, size); + case AVDTP_GET_CONFIGURATION: + debug("Received GET_CONFIGURATION_CMD"); + return avdtp_getconf_cmd(session, (void *) header, size); + case AVDTP_RECONFIGURE: + debug("Received RECONFIGURE_CMD"); + return avdtp_reconf_cmd(session, (void *) header, size); + case AVDTP_OPEN: + debug("Received OPEN_CMD"); + return avdtp_open_cmd(session, (void *) header, size); + case AVDTP_START: + debug("Received START_CMD"); + return avdtp_start_cmd(session, (void *) header, size); + case AVDTP_CLOSE: + debug("Received CLOSE_CMD"); + return avdtp_close_cmd(session, (void *) header, size); + case AVDTP_SUSPEND: + debug("Received SUSPEND_CMD"); + return avdtp_suspend_cmd(session, (void *) header, size); + case AVDTP_ABORT: + debug("Received ABORT_CMD"); + return avdtp_abort_cmd(session, (void *) header, size); + case AVDTP_SECURITY_CONTROL: + debug("Received SECURITY_CONTROL_CMD"); + return avdtp_secctl_cmd(session, (void *) header, size); + default: + debug("Received unknown request id %u", header->signal_id); + return avdtp_unknown_cmd(session, (void *) header, size); + } +} + +static gboolean transport_cb(GIOChannel *chan, GIOCondition cond, + gpointer data) +{ + struct avdtp_stream *stream = data; + struct avdtp_local_sep *sep = stream->lsep; + + if (stream->close_int && sep->cfm && sep->cfm->close) + sep->cfm->close(sep, stream); + + avdtp_sep_set_state(stream->session, sep, AVDTP_STATE_IDLE); + + return FALSE; +} + +static void handle_transport_connect(struct avdtp *session, int sock, + uint16_t mtu) +{ + struct avdtp_stream *stream = session->pending_open; + struct avdtp_local_sep *sep = stream->lsep; + GIOChannel *channel; + + session->pending_open = NULL; + + stream->sock = sock; + stream->mtu = mtu; + + if (sep->cfm && sep->cfm->open) + sep->cfm->open(sep, stream); + + channel = g_io_channel_unix_new(stream->sock); + + stream->io = g_io_add_watch(channel, G_IO_ERR | G_IO_HUP | G_IO_NVAL, + (GIOFunc) transport_cb, stream); + g_io_channel_unref(channel); + + avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN); +} + +static void init_request(struct avdtp_header *header, int request_id) +{ + static int transaction = 0; + + header->packet_type = AVDTP_PKT_TYPE_SINGLE; + header->message_type = AVDTP_MSG_TYPE_COMMAND; + header->transaction = transaction; + header->signal_id = request_id; + + /* clear rfa bits */ + header->rfa0 = 0; + + transaction = (transaction + 1) % 16; +} + +static gboolean session_cb(GIOChannel *chan, GIOCondition cond, + gpointer data) +{ + struct avdtp *session = data; + struct avdtp_header *header; + gsize size; + + debug("session_cb"); + + if (cond & G_IO_NVAL) + return FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR)) + goto failed; + + if (g_io_channel_read(chan, session->buf, session->mtu, + &size) != G_IO_ERROR_NONE) { + error("IO Channel read error"); + goto failed; + } + + if (size < sizeof(struct avdtp_header)) { + error("Received too small packet (%d bytes)", size); + goto failed; + } + + header = (struct avdtp_header *) session->buf; + + if (header->message_type == AVDTP_MSG_TYPE_COMMAND) { + if (!avdtp_parse_cmd(session, header, size)) { + error("Unable to handle command. Disconnecting"); + goto failed; + } + + if (session->ref == 1 && !session->streams) + set_disconnect_timer(session); + + return TRUE; + } + + if (session->req == NULL) { + error("No pending request, rejecting message"); + return TRUE; + } + + if (header->transaction != session->req->msg->transaction) { + error("Transaction label doesn't match"); + return TRUE; + } + + if (header->signal_id != session->req->msg->signal_id) { + error("Reponse signal doesn't match"); + return TRUE; + } + + g_source_remove(session->req->timeout); + session->req->timeout = 0; + + switch(header->message_type) { + case AVDTP_MSG_TYPE_ACCEPT: + if (!avdtp_parse_resp(session, session->req->stream, header, + size)) { + error("Unable to parse accept response"); + goto failed; + } + break; + case AVDTP_MSG_TYPE_REJECT: + if (!avdtp_parse_rej(session, session->req->stream, header, + size)) { + error("Unable to parse reject response"); + goto failed; + } + break; + default: + error("Unknown message type"); + break; + } + + pending_req_free(session->req); + session->req = NULL; + + process_queue(session); + + return TRUE; + +failed: + connection_lost(session); + avdtp_unref(session); + return FALSE; +} + +static gboolean l2cap_connect_cb(GIOChannel *chan, GIOCondition cond, + gpointer data) +{ + struct avdtp *session = data; + struct l2cap_options l2o; + socklen_t len; + int ret, err, sk; + char address[18]; + + if (cond & G_IO_NVAL) + return FALSE; + + if (!g_slist_find(sessions, session)) { + debug("l2cap_connect_cb: session got removed"); + return FALSE; + } + + if (cond & (G_IO_ERR | G_IO_HUP)) { + err = EIO; + goto failed; + } + + sk = g_io_channel_unix_get_fd(chan); + len = sizeof(ret); + if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { + err = errno; + error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err); + goto failed; + } + + if (ret != 0) { + err = ret; + error("connect(): %s (%d)", strerror(err), err); + goto failed; + } + + ba2str(&session->dst, address); + debug("AVDTP: connected %s channel to %s", + session->pending_open ? "transport" : "signaling", + address); + + memset(&l2o, 0, sizeof(l2o)); + len = sizeof(l2o); + if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, + &len) < 0) { + err = errno; + error("getsockopt(L2CAP_OPTIONS): %s (%d)", strerror(err), + err); + goto failed; + } + + if (session->state == AVDTP_SESSION_STATE_CONNECTING) { + session->sock = sk; + session->mtu = l2o.imtu; + session->buf = g_malloc0(session->mtu); + session->state = AVDTP_SESSION_STATE_CONNECTED; + session->io = g_io_add_watch(chan, + G_IO_IN | G_IO_ERR | G_IO_HUP + | G_IO_NVAL, + (GIOFunc) session_cb, session); + } + else if (session->pending_open) + handle_transport_connect(session, sk, l2o.imtu); + + process_queue(session); + + return FALSE; + +failed: + if (session->pending_open) { + avdtp_sep_set_state(session, session->pending_open->lsep, + AVDTP_STATE_IDLE); + session->pending_open = NULL; + } else { + finalize_discovery(session, -err); + connection_lost(session); + avdtp_unref(session); + } + + return FALSE; +} + +static int l2cap_connect(struct avdtp *session) +{ + struct sockaddr_l2 l2a; + GIOChannel *io; + int sk; + + memset(&l2a, 0, sizeof(l2a)); + l2a.l2_family = AF_BLUETOOTH; + bacpy(&l2a.l2_bdaddr, &session->src); + + sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + if (sk < 0) { + error("Cannot create L2CAP socket. %s(%d)", strerror(errno), + errno); + return -errno; + } + + if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) { + error("Bind failed. %s (%d)", strerror(errno), errno); + return -errno; + } + + memset(&l2a, 0, sizeof(l2a)); + l2a.l2_family = AF_BLUETOOTH; + bacpy(&l2a.l2_bdaddr, &session->dst); + l2a.l2_psm = htobs(AVDTP_PSM); + + if (set_nonblocking(sk) < 0) { + error("Set non blocking: %s (%d)", strerror(errno), errno); + return -errno; + } + + io = g_io_channel_unix_new(sk); + g_io_channel_set_close_on_unref(io, FALSE); + + if (connect(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) { + if (!(errno == EAGAIN || errno == EINPROGRESS)) { + error("Connect failed. %s(%d)", strerror(errno), + errno); + finalize_discovery(session, errno); + g_io_channel_close(io); + g_io_channel_unref(io); + return -errno; + } + g_io_add_watch(io, G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + (GIOFunc) l2cap_connect_cb, session); + + if (session->state == AVDTP_SESSION_STATE_DISCONNECTED) + session->state = AVDTP_SESSION_STATE_CONNECTING; + } else + l2cap_connect_cb(io, G_IO_OUT, session); + + g_io_channel_unref(io); + + return 0; +} + +static void queue_request(struct avdtp *session, struct pending_req *req, + gboolean priority) +{ + if (priority) + session->prio_queue = g_slist_append(session->prio_queue, req); + else + session->req_queue = g_slist_append(session->req_queue, req); +} + +static struct avdtp_remote_sep *find_remote_sep(GSList *seps, uint8_t seid) +{ + GSList *l; + + for (l = seps; l != NULL; l = g_slist_next(l)) { + struct avdtp_remote_sep *sep = l->data; + + if (sep->seid == seid) + return sep; + } + + return NULL; +} + +static gboolean request_timeout(gpointer user_data) +{ + struct avdtp *session = user_data; + struct pending_req *req; + struct seid_req sreq; + struct avdtp_remote_sep *sep; + struct avdtp_stream *stream; + uint8_t seid; + + error("Request timed out"); + + req = session->req; + session->req = NULL; + + switch (req->msg->signal_id) { + case AVDTP_DISCOVER: + case AVDTP_GET_CAPABILITIES: + case AVDTP_SET_CONFIGURATION: + case AVDTP_ABORT: + goto failed; + } + + seid = ((struct seid_req *) (req->msg))->acp_seid; + + sep = find_remote_sep(session->seps, seid); + if (!sep) { + error("Unable to find matching remote SEID %u", seid); + goto failed; + } + + stream = sep->stream; + + memset(&sreq, 0, sizeof(sreq)); + init_request(&sreq.header, AVDTP_ABORT); + sreq.acp_seid = seid; + + if (!send_request(session, TRUE, stream, &sreq, sizeof(sreq))) { + error("Unable to send abort request"); + goto failed; + } + + goto done; + +failed: + connection_lost(session); + avdtp_unref(session); +done: + pending_req_free(req); + return FALSE; +} + +static int send_req(struct avdtp *session, gboolean priority, + struct pending_req *req) +{ + int err; + + if (session->state == AVDTP_SESSION_STATE_DISCONNECTED) { + err = l2cap_connect(session); + if (err < 0) + goto failed; + } + + if (session->state < AVDTP_SESSION_STATE_CONNECTED || + session->req != NULL) { + queue_request(session, req, priority); + return 0; + } + + /* FIXME: Should we retry to send if the buffer + was not totally sent or in case of EINTR? */ + err = avdtp_send(session, req->msg, req->msg_size); + if (err < 0) + goto failed; + + session->req = req; + + req->timeout = g_timeout_add(REQ_TIMEOUT, request_timeout, + session); + return 0; + +failed: + g_free(req->msg); + g_free(req); + return err; +} + +static int send_request(struct avdtp *session, gboolean priority, + struct avdtp_stream *stream, void *buffer, int size) +{ + struct pending_req *req; + + req = g_new0(struct pending_req, 1); + req->msg = g_malloc(size); + memcpy(req->msg, buffer, size); + req->msg_size = size; + req->stream = stream; + + return send_req(session, priority, req); +} + +static gboolean avdtp_discover_resp(struct avdtp *session, + struct discover_resp *resp, int size) +{ + int sep_count, i, isize = sizeof(struct seid_info); + + sep_count = (size - sizeof(struct avdtp_header)) / isize; + + for (i = 0; i < sep_count; i++) { + struct avdtp_remote_sep *sep; + struct seid_req req; + int ret; + + /* Skip SEP's which are in use */ + if (resp->seps[i].inuse) + continue; + + sep = find_remote_sep(session->seps, resp->seps[i].seid); + if (!sep) { + sep = g_new0(struct avdtp_remote_sep, 1); + session->seps = g_slist_append(session->seps, sep); + } + else if (sep && sep->stream) + continue; + + sep->seid = resp->seps[i].seid; + sep->type = resp->seps[i].type; + sep->media_type = resp->seps[i].media_type; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_GET_CAPABILITIES); + req.acp_seid = sep->seid; + + ret = send_request(session, TRUE, NULL, &req, sizeof(req)); + if (ret < 0) { + finalize_discovery(session, ret); + break; + } + } + + return TRUE; +} + +static gboolean avdtp_get_capabilities_resp(struct avdtp *session, + struct getcap_resp *resp, + int size) +{ + int processed; + struct avdtp_remote_sep *sep; + unsigned char *ptr; + uint8_t seid; + + /* Check for minimum required packet size includes: + * 1. getcap resp header + * 2. media transport capability (2 bytes) + * 3. media codec capability type + length (2 bytes) + * 4. the actual media codec elements + * */ + if (size < (sizeof(struct getcap_resp) + 4 + + sizeof(struct avdtp_media_codec_capability))) { + error("Too short getcap resp packet"); + return FALSE; + } + + seid = ((struct seid_req *) session->req->msg)->acp_seid; + + sep = find_remote_sep(session->seps, seid); + + if (sep->caps) { + g_slist_foreach(sep->caps, (GFunc) g_free, NULL); + g_slist_free(sep->caps); + sep->caps = NULL; + sep->codec = NULL; + } + + ptr = resp->caps; + processed = sizeof(struct getcap_resp); + + while (processed + 2 < size) { + struct avdtp_service_capability *cap; + uint8_t length, category; + + category = ptr[0]; + length = ptr[1]; + + if (processed + 2 + length > size) { + error("Invalid capability data in getcap resp"); + return FALSE; + } + + cap = g_malloc(sizeof(struct avdtp_service_capability) + + length); + memcpy(cap, ptr, 2 + length); + + processed += 2 + length; + ptr += 2 + length; + + sep->caps = g_slist_append(sep->caps, cap); + + if (category == AVDTP_MEDIA_CODEC && + length >= + sizeof(struct avdtp_media_codec_capability)) + sep->codec = cap; + + } + + return TRUE; +} + +static gboolean avdtp_set_configuration_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct avdtp_header *resp, + int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + if (sep->cfm && sep->cfm->set_configuration) + sep->cfm->set_configuration(sep, stream); + + avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED); + + return TRUE; +} + +static gboolean avdtp_reconfigure_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct avdtp_header *resp, int size) +{ + return TRUE; +} + +static gboolean avdtp_open_resp(struct avdtp *session, struct avdtp_stream *stream, + struct seid_rej *resp, int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + if (l2cap_connect(session) < 0) + avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE); + + session->pending_open = stream; + + return TRUE; +} + +static gboolean avdtp_start_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct seid_rej *resp, int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + if (sep->cfm && sep->cfm->start) + sep->cfm->start(sep, stream); + + avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING); + + return TRUE; +} + +static gboolean avdtp_close_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct seid_rej *resp, int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING); + + close(stream->sock); + stream->sock = -1; + + return TRUE; +} + +static gboolean avdtp_suspend_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct stream_pause_resp *resp, + int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + if (sep->cfm && sep->cfm->suspend) + sep->cfm->suspend(sep, stream); + + avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN); + + return TRUE; +} + +static gboolean avdtp_abort_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct seid_rej *resp, int size) +{ + struct avdtp_local_sep *sep = stream->lsep; + + if (sep->cfm && sep->cfm->suspend) + sep->cfm->suspend(sep, stream); + + avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE); + + return TRUE; +} + +static gboolean avdtp_parse_resp(struct avdtp *session, + struct avdtp_stream *stream, + struct avdtp_header *header, int size) +{ + struct avdtp_header *next; + + if (session->prio_queue) + next = ((struct pending_req *) + (session->prio_queue->data))->msg; + else if (session->req_queue) + next = ((struct pending_req *) + (session->req_queue->data))->msg; + else + next = NULL; + + switch (header->signal_id) { + case AVDTP_DISCOVER: + debug("DISCOVER request succeeded"); + return avdtp_discover_resp(session, (void *) header, size); + case AVDTP_GET_CAPABILITIES: + debug("GET_CAPABILITIES request succeeded"); + if (!avdtp_get_capabilities_resp(session, (void *) header, + size)) + return FALSE; + if (!(next && next->signal_id == AVDTP_GET_CAPABILITIES)) + finalize_discovery(session, 0); + return TRUE; + case AVDTP_SET_CONFIGURATION: + debug("SET_CONFIGURATION request succeeded"); + return avdtp_set_configuration_resp(session, stream, + (void *) header, size); + case AVDTP_RECONFIGURE: + debug("RECONFIGURE request succeeded"); + return avdtp_reconfigure_resp(session, stream, (void *) header, + size); + case AVDTP_OPEN: + debug("OPEN request succeeded"); + return avdtp_open_resp(session, stream, (void *) header, size); + case AVDTP_SUSPEND: + debug("SUSPEND request succeeded"); + return avdtp_suspend_resp(session, stream, (void *) header, + size); + case AVDTP_START: + debug("START request succeeded"); + return avdtp_start_resp(session, stream, (void *) header, + size); + case AVDTP_CLOSE: + debug("CLOSE request succeeded"); + return avdtp_close_resp(session, stream, (void *) header, + size); + case AVDTP_ABORT: + debug("ABORT request succeeded"); + return avdtp_abort_resp(session, stream, (void *) header, + size); + } + + error("Unknown signal id in accept response: %u", header->signal_id); + + return TRUE; +} + +static gboolean seid_rej_to_err(struct seid_rej *rej, int size, + struct avdtp_error *err) +{ + if (size < sizeof(struct seid_rej)) { + error("Too small packet for seid_rej"); + return FALSE; + } + + avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error); + + return TRUE; +} + +static gboolean conf_rej_to_err(struct conf_rej *rej, int size, + struct avdtp_error *err, uint8_t *category) +{ + if (size < sizeof(struct conf_rej)) { + error("Too small packet for conf_rej"); + return FALSE; + } + + avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error); + + if (category) + *category = rej->category; + + return TRUE; +} + +static gboolean stream_rej_to_err(struct stream_rej *rej, int size, + struct avdtp_error *err, uint8_t *acp_seid) +{ + if (size < sizeof(struct conf_rej)) { + error("Too small packet for stream_rej"); + return FALSE; + } + + avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error_code); + + if (acp_seid) + *acp_seid = rej->acp_seid; + + return TRUE; +} + +static gboolean avdtp_parse_rej(struct avdtp *session, struct avdtp_stream *stream, + struct avdtp_header *header, int size) +{ + struct avdtp_error err; + uint8_t acp_seid, category; + + switch (header->signal_id) { + case AVDTP_DISCOVER: + if (!seid_rej_to_err((void *) header, size, &err)) + return FALSE; + error("DISCOVER request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_GET_CAPABILITIES: + if (!seid_rej_to_err((void *) header, size, &err)) + return FALSE; + error("GET_CAPABILITIES request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_OPEN: + if (!seid_rej_to_err((void *) header, size, &err)) + return FALSE; + error("OPEN request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_SET_CONFIGURATION: + if (!conf_rej_to_err((void *) header, size, &err, &category)) + return FALSE; + error("SET_CONFIGURATION request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_RECONFIGURE: + if (!conf_rej_to_err((void *) header, size, &err, &category)) + return FALSE; + error("RECONFIGURE request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_START: + if (!stream_rej_to_err((void *) header, size, &err, &acp_seid)) + return FALSE; + error("START request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_SUSPEND: + if (!stream_rej_to_err((void *) header, size, &err, &acp_seid)) + return FALSE; + error("SUSPEND request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_CLOSE: + if (!stream_rej_to_err((void *) header, size, &err, &acp_seid)) + return FALSE; + error("CLOSE request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + case AVDTP_ABORT: + if (!stream_rej_to_err((void *) header, size, &err, &acp_seid)) + return FALSE; + error("ABORT request rejected: %s (%d)", + avdtp_strerror(&err), err.err.error_code); + return TRUE; + default: + error("Unknown reject response signal id: %u", + header->signal_id); + return TRUE; + } +} + +static struct avdtp *find_session(bdaddr_t *src, bdaddr_t *dst) +{ + GSList *l; + + for (l = sessions; l != NULL; l = g_slist_next(l)) { + struct avdtp *s = l->data; + + if (bacmp(src, &s->src) || bacmp(dst, &s->dst)) + continue; + + return s; + } + + return NULL; +} + +struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst) +{ + struct avdtp *session; + + assert(src != NULL); + assert(dst != NULL); + + session = find_session(src, dst); + if (session) + return avdtp_ref(session); + + session = g_new0(struct avdtp, 1); + + session->sock = -1; + bacpy(&session->src, src); + bacpy(&session->dst, dst); + session->ref = 1; + session->state = AVDTP_SESSION_STATE_DISCONNECTED; + + sessions = g_slist_append(sessions, session); + + return avdtp_ref(session); +} + +gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock, + uint16_t *mtu, GSList **caps) +{ + if (stream->sock < 0) + return FALSE; + + if (sock) + *sock = stream->sock; + + if (mtu) + *mtu = stream->mtu; + + if (caps) + *caps = stream->caps; + + return TRUE; +} + +static int process_queue(struct avdtp *session) +{ + GSList **queue, *l; + struct pending_req *req; + + if (session->req) + return 0; + + if (session->prio_queue) + queue = &session->prio_queue; + else + queue = &session->req_queue; + + if (!*queue) + return 0; + + l = *queue; + req = l->data; + + *queue = g_slist_remove(*queue, req); + + return send_req(session, FALSE, req); +} + +struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep) +{ + return sep->codec; +} + +struct avdtp_service_capability *avdtp_service_cap_new(uint8_t category, + void *data, int length) +{ + struct avdtp_service_capability *cap; + + cap = g_malloc(sizeof(struct avdtp_service_capability) + length); + cap->category = category; + cap->length = length; + memcpy(cap->data, data, length); + + return cap; +} + +int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb, void *user_data) +{ + struct discover_req req; + int ret; + + if (session->discov_cb) + return -EBUSY; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_DISCOVER); + + ret = send_request(session, FALSE, NULL, &req, sizeof(req)); + if (ret == 0) { + session->discov_cb = cb; + session->user_data = user_data; + } + + return ret; +} + +int avdtp_get_seps(struct avdtp *session, uint8_t acp_type, uint8_t media_type, + uint8_t codec, struct avdtp_local_sep **lsep, + struct avdtp_remote_sep **rsep) +{ + GSList *l; + uint8_t int_type; + + int_type = acp_type == AVDTP_SEP_TYPE_SINK ? + AVDTP_SEP_TYPE_SOURCE : AVDTP_SEP_TYPE_SINK; + + *lsep = find_local_sep(int_type, media_type, codec); + if (!*lsep) + return -EINVAL; + + for (l = session->seps; l != NULL; l = g_slist_next(l)) { + struct avdtp_remote_sep *sep = l->data; + struct avdtp_service_capability *cap; + struct avdtp_media_codec_capability *codec_data; + + if (sep->type != acp_type) + continue; + + if (sep->media_type != media_type) + continue; + + if (!sep->codec) + continue; + + cap = sep->codec; + codec_data = (void *) cap->data; + + if (codec_data->media_codec_type != codec) + continue; + + if (!sep->stream) { + *rsep = sep; + return 0; + } + } + + return -EINVAL; +} + +void avdtp_stream_set_cb(struct avdtp *session, struct avdtp_stream *stream, + avdtp_stream_state_cb cb, void *data) +{ + stream->cb = cb; + stream->user_data = data; +} + +int avdtp_get_configuration(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + + if (session->state < AVDTP_SESSION_STATE_CONNECTED) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_GET_CONFIGURATION); + req.acp_seid = stream->rsep->seid; + + return send_request(session, FALSE, stream, &req, sizeof(req)); +} + +int avdtp_set_configuration(struct avdtp *session, + struct avdtp_remote_sep *rsep, + struct avdtp_local_sep *lsep, + GSList *caps, + struct avdtp_stream **stream) +{ + struct setconf_req *req; + struct avdtp_stream *new_stream; + unsigned char *ptr; + int ret, caps_len; + struct avdtp_service_capability *cap; + GSList *l; + + if (session->state != AVDTP_SESSION_STATE_CONNECTED) + return -ENOTCONN; + + if (!(lsep && rsep)) + return -EINVAL; + + new_stream = g_new0(struct avdtp_stream, 1); + + new_stream->session = session; + new_stream->lsep = lsep; + new_stream->rsep = rsep; + new_stream->caps = caps; + + /* Calculate total size of request */ + for (l = caps, caps_len = 0; l != NULL; l = g_slist_next(l)) { + cap = l->data; + caps_len += cap->length + 2; + } + + req = g_malloc0(sizeof(struct setconf_req) + caps_len); + + init_request(&req->header, AVDTP_SET_CONFIGURATION); + req->acp_seid = lsep->info.seid; + req->int_seid = rsep->seid; + + /* Copy the capabilities into the request */ + for (l = caps, ptr = req->caps; l != NULL; l = g_slist_next(l)) { + cap = l->data; + memcpy(ptr, cap, cap->length + 2); + ptr += cap->length + 2; + } + + ret = send_request(session, FALSE, new_stream, req, + sizeof(struct setconf_req) + caps_len); + if (ret < 0) + stream_free(new_stream); + else { + lsep->info.inuse = 1; + lsep->stream = new_stream; + rsep->stream = new_stream; + session->streams = g_slist_append(session->streams, new_stream); + if (stream) + *stream = new_stream; + } + + g_free(req); + + return ret; +} + +int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state != AVDTP_STATE_OPEN) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_GET_CONFIGURATION); + req.acp_seid = stream->rsep->seid; + + return send_request(session, FALSE, NULL, &req, sizeof(req)); +} + +int avdtp_open(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state > AVDTP_STATE_CONFIGURED) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_OPEN); + req.acp_seid = stream->rsep->seid; + + return send_request(session, FALSE, stream, &req, sizeof(req)); +} + +int avdtp_start(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state != AVDTP_STATE_OPEN) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_START); + req.acp_seid = stream->rsep->seid; + + return send_request(session, FALSE, stream, &req, sizeof(req)); +} + +int avdtp_close(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + int ret; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state < AVDTP_STATE_OPEN) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_CLOSE); + req.acp_seid = stream->rsep->seid; + + ret = send_request(session, FALSE, stream, &req, sizeof(req)); + if (ret == 0) + stream->close_int = TRUE; + + return ret; +} + +int avdtp_suspend(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + int ret; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state <= AVDTP_STATE_OPEN) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_SUSPEND); + req.acp_seid = stream->rsep->seid; + + ret = send_request(session, FALSE, stream, &req, sizeof(req)); + if (ret == 0) + avdtp_sep_set_state(session, stream->lsep, + AVDTP_STATE_OPEN); + + return ret; +} + +int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream) +{ + struct seid_req req; + int ret; + + if (!g_slist_find(session->streams, stream)) + return -EINVAL; + + if (stream->lsep->state <= AVDTP_STATE_OPEN) + return -EINVAL; + + memset(&req, 0, sizeof(req)); + init_request(&req.header, AVDTP_ABORT); + req.acp_seid = stream->rsep->seid; + + ret = send_request(session, FALSE, stream, &req, sizeof(req)); + if (ret == 0) + avdtp_sep_set_state(session, stream->lsep, + AVDTP_STATE_ABORTING); + + return 0; +} + +struct avdtp_local_sep *avdtp_register_sep(uint8_t type, uint8_t media_type, + struct avdtp_sep_ind *ind, + struct avdtp_sep_cfm *cfm) +{ + struct avdtp_local_sep *sep; + + if (free_seid > MAX_SEID) + return NULL; + + sep = g_new0(struct avdtp_local_sep, 1); + + sep->state = AVDTP_STATE_IDLE; + sep->info.seid = free_seid++; + sep->info.type = type; + sep->info.media_type = media_type; + sep->ind = ind; + sep->cfm = cfm; + + local_seps = g_slist_append(local_seps, sep); + + return sep; +} + +int avdtp_unregister_sep(struct avdtp_local_sep *sep) +{ + if (!sep) + return -EINVAL; + + if (sep->info.inuse) + return -EBUSY; + + local_seps = g_slist_remove(local_seps, sep); + + g_free(sep); + + return 0; +} + +static gboolean avdtp_server_cb(GIOChannel *chan, GIOCondition cond, void *data) +{ + int srv_sk, cli_sk; + socklen_t size; + struct sockaddr_l2 addr; + struct l2cap_options l2o; + bdaddr_t src, dst; + struct avdtp *session; + GIOChannel *cli_io; + char address[18]; + + if (cond & G_IO_NVAL) + return FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR)) { + error("Hangup or error on AVDTP server socket"); + g_io_channel_close(chan); + raise(SIGTERM); + return FALSE; + } + + srv_sk = g_io_channel_unix_get_fd(chan); + + size = sizeof(struct sockaddr_l2); + cli_sk = accept(srv_sk, (struct sockaddr *) &addr, &size); + if (cli_sk < 0) { + error("AVDTP accept: %s (%d)", strerror(errno), errno); + return TRUE; + } + + bacpy(&dst, &addr.l2_bdaddr); + + ba2str(&dst, address); + debug("AVDTP: incoming connect from %s", address); + + size = sizeof(struct sockaddr_l2); + if (getsockname(srv_sk, (struct sockaddr *) &addr, &size) < 0) { + error("getsockname: %s (%d)", strerror(errno), errno); + close(cli_sk); + return TRUE; + } + + bacpy(&src, &addr.l2_bdaddr); + + memset(&l2o, 0, sizeof(l2o)); + size = sizeof(l2o); + if (getsockopt(cli_sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &size) < 0) { + error("getsockopt(L2CAP_OPTIONS): %s (%d)", strerror(errno), + errno); + close(cli_sk); + return TRUE; + } + + session = avdtp_get(&src, &dst); + + if (session->pending_open && session->pending_open->open_acp) { + handle_transport_connect(session, cli_sk, l2o.imtu); + return TRUE; + } + + if (session->sock >= 0) { + error("Refusing unexpected connect from %s", address); + close(cli_sk); + return TRUE; + } + + if (session->ref == 1) + set_disconnect_timer(session); + + session->mtu = l2o.imtu; + session->buf = g_malloc0(session->mtu); + session->sock = cli_sk; + session->state = AVDTP_SESSION_STATE_CONNECTED; + + cli_io = g_io_channel_unix_new(session->sock); + session->io = g_io_add_watch(cli_io, + G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL, + (GIOFunc) session_cb, session); + g_io_channel_unref(cli_io); + + return TRUE; +} + +static GIOChannel *avdtp_server_socket(void) +{ + int sock, lm; + struct sockaddr_l2 addr; + GIOChannel *io; + + sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + if (sock < 0) { + error("AVDTP server socket: %s (%d)", strerror(errno), errno); + return NULL; + } + + lm = L2CAP_LM_SECURE; + if (setsockopt(sock, SOL_L2CAP, L2CAP_LM, &lm, sizeof(lm)) < 0) { + error("AVDTP server setsockopt: %s (%d)", strerror(errno), errno); + close(sock); + return NULL; + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + bacpy(&addr.l2_bdaddr, BDADDR_ANY); + addr.l2_psm = htobs(AVDTP_PSM); + + if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + error("AVDTP server bind: %s", strerror(errno), errno); + close(sock); + return NULL; + } + + if (listen(sock, 4) < 0) { + error("AVDTP server listen: %s", strerror(errno), errno); + close(sock); + return NULL; + } + + io = g_io_channel_unix_new(sock); + if (!io) { + error("Unable to allocate new io channel"); + close(sock); + return NULL; + } + + return io; +} + +const char *avdtp_strerror(struct avdtp_error *err) +{ + if (err->type == AVDTP_ERROR_ERRNO) + return strerror(err->err.posix_errno); + + switch(err->err.error_code) { + case AVDTP_BAD_HEADER_FORMAT: + return "Bad Header Format"; + case AVDTP_BAD_LENGTH: + return "Bad Packet Lenght"; + case AVDTP_BAD_ACP_SEID: + return "Bad Acceptor SEID"; + case AVDTP_SEP_IN_USE: + return "Stream End Point in Use"; + case AVDTP_SEP_NOT_IN_USE: + return "Stream End Point Not in Use"; + case AVDTP_BAD_SERV_CATEGORY: + return "Bad Service Category"; + case AVDTP_BAD_PAYLOAD_FORMAT: + return "Bad Payload format"; + case AVDTP_NOT_SUPPORTED_COMMAND: + return "Command Not Supported"; + case AVDTP_INVALID_CAPABILITIES: + return "Invalid Capabilities"; + case AVDTP_BAD_RECOVERY_TYPE: + return "Bad Recovery Type"; + case AVDTP_BAD_MEDIA_TRANSPORT_FORMAT: + return "Bad Media Transport Format"; + case AVDTP_BAD_RECOVERY_FORMAT: + return "Bad Recovery Format"; + case AVDTP_BAD_ROHC_FORMAT: + return "Bad Header Compression Format"; + case AVDTP_BAD_CP_FORMAT: + return "Bad Content Protetion Format"; + case AVDTP_BAD_MULTIPLEXING_FORMAT: + return "Bad Multiplexing Format"; + case AVDTP_UNSUPPORTED_CONFIGURATION: + return "Configuration not supported"; + case AVDTP_BAD_STATE: + return "Bad State"; + default: + return "Unknow error"; + } +} + +int avdtp_init(void) +{ + if (avdtp_server) + return 0; + + avdtp_server = avdtp_server_socket(); + if (!avdtp_server) + return -1; + + g_io_add_watch(avdtp_server, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + (GIOFunc) avdtp_server_cb, NULL); + + return 0; +} + +void avdtp_exit(void) +{ + if (!avdtp_server) + return; + + g_io_channel_close(avdtp_server); + g_io_channel_unref(avdtp_server); + avdtp_server = NULL; +} diff --git a/audio/avdtp.h b/audio/avdtp.h index 153a1731..b4af9eb3 100644 --- a/audio/avdtp.h +++ b/audio/avdtp.h @@ -20,4 +20,181 @@ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * */ +#ifndef __AVDTP_H__ +#define __AVDTP_H__ +#include <stdint.h> +#include <bluetooth/bluetooth.h> + +struct avdtp; +struct avdtp_stream; +struct avdtp_local_sep; +struct avdtp_remote_sep; +struct avdtp_error; + +/* SEP capability categories */ +#define AVDTP_MEDIA_TRANSPORT 0x01 +#define AVDTP_REPORTING 0x02 +#define AVDTP_RECOVERY 0x03 +#define AVDTP_CONTENT_PROTECTION 0x04 +#define AVDTP_HEADER_COMPRESSION 0x05 +#define AVDTP_MULTIPLEXING 0x06 +#define AVDTP_MEDIA_CODEC 0x07 + +/* AVDTP error definitions */ +#define AVDTP_BAD_HEADER_FORMAT 0x01 +#define AVDTP_BAD_LENGTH 0x11 +#define AVDTP_BAD_ACP_SEID 0x12 +#define AVDTP_SEP_IN_USE 0x13 +#define AVDTP_SEP_NOT_IN_USE 0x14 +#define AVDTP_BAD_SERV_CATEGORY 0x17 +#define AVDTP_BAD_PAYLOAD_FORMAT 0x18 +#define AVDTP_NOT_SUPPORTED_COMMAND 0x19 +#define AVDTP_INVALID_CAPABILITIES 0x1A +#define AVDTP_BAD_RECOVERY_TYPE 0x22 +#define AVDTP_BAD_MEDIA_TRANSPORT_FORMAT 0x23 +#define AVDTP_BAD_RECOVERY_FORMAT 0x25 +#define AVDTP_BAD_ROHC_FORMAT 0x26 +#define AVDTP_BAD_CP_FORMAT 0x27 +#define AVDTP_BAD_MULTIPLEXING_FORMAT 0x28 +#define AVDTP_UNSUPPORTED_CONFIGURATION 0x29 +#define AVDTP_BAD_STATE 0x31 + +/* SEP types definitions */ +#define AVDTP_SEP_TYPE_SOURCE 0x00 +#define AVDTP_SEP_TYPE_SINK 0x01 + +/* Media types definitions */ +#define AVDTP_MEDIA_TYPE_AUDIO 0x00 +#define AVDTP_MEDIA_TYPE_VIDEO 0x01 +#define AVDTP_MEDIA_TYPE_MULTIMEDIA 0x02 + +typedef enum { + AVDTP_STATE_IDLE, + AVDTP_STATE_CONFIGURED, + AVDTP_STATE_OPEN, + AVDTP_STATE_STREAMING, + AVDTP_STATE_CLOSING, + AVDTP_STATE_ABORTING, +} avdtp_state_t; + +struct avdtp_service_capability { + uint8_t category; + uint8_t length; + uint8_t data[0]; +} __attribute__ ((packed)); + +struct avdtp_media_codec_capability { + uint8_t rfa0:4; + uint8_t media_type:4; + uint8_t media_codec_type; + uint8_t data[0]; +} __attribute__ ((packed)); + +typedef void (*avdtp_stream_state_cb) (struct avdtp_stream *stream, + avdtp_state_t old_state, + avdtp_state_t new_state, + struct avdtp_error *err, + void *user_data); + +/* Callbacks for when a reply is received to a command that we sent */ +struct avdtp_sep_cfm { + void (*set_configuration) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*get_configuration) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*open) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*start) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*suspend) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*close) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*abort) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream); + void (*reconfigure) (struct avdtp_local_sep *lsep); +}; + +/* Callbacks for indicating when we received a new command. The return value + * indicates whether the command should be rejected or accepted */ +struct avdtp_sep_ind { + gboolean (*get_capability) (struct avdtp_local_sep *sep, + GSList **caps, uint8_t *err); + gboolean (*set_configuration) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream, + uint8_t int_seid, GSList *caps, + uint8_t *err); + gboolean (*get_configuration) (struct avdtp_local_sep *lsep, + uint8_t *err); + gboolean (*open) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream, uint8_t *err); + gboolean (*start) (struct avdtp_local_sep *lsep, + struct avdtp_stream *stream, + uint8_t *err); + gboolean (*suspend) (struct avdtp_local_sep *sep, + struct avdtp_stream *stream, + uint8_t *err); + gboolean (*close) (struct avdtp_local_sep *sep, + struct avdtp_stream *stream, + uint8_t *err); + gboolean (*abort) (struct avdtp_local_sep *sep, + struct avdtp_stream *stream, uint8_t *err); + gboolean (*reconfigure) (struct avdtp_local_sep *lsep, uint8_t *err); +}; + +typedef void (*avdtp_discover_cb_t) (struct avdtp *session, GSList *seps, + int err, void *user_data); + +struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst); + +void avdtp_unref(struct avdtp *session); +struct avdtp *avdtp_ref(struct avdtp *session); + +struct avdtp_service_capability *avdtp_service_cap_new(uint8_t category, + void *data, int size); + +struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep); + +int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb, + void *user_data); + +void avdtp_stream_set_cb(struct avdtp *session, struct avdtp_stream *stream, + avdtp_stream_state_cb cb, void *data); + +gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock, + uint16_t *mtu, GSList **caps); + +int avdtp_set_configuration(struct avdtp *session, + struct avdtp_remote_sep *rsep, + struct avdtp_local_sep *lsep, + GSList *caps, + struct avdtp_stream **stream); + +int avdtp_get_configuration(struct avdtp *session, + struct avdtp_stream *stream); + +int avdtp_open(struct avdtp *session, struct avdtp_stream *stream); +int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream); +int avdtp_start(struct avdtp *session, struct avdtp_stream *stream); +int avdtp_suspend(struct avdtp *session, struct avdtp_stream *stream); +int avdtp_close(struct avdtp *session, struct avdtp_stream *stream); +int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream); + +struct avdtp_local_sep *avdtp_register_sep(uint8_t type, uint8_t media_type, + struct avdtp_sep_ind *ind, + struct avdtp_sep_cfm *cfm); + +/* Find a matching pair of local and remote SEP ID's */ +int avdtp_get_seps(struct avdtp *session, uint8_t type, uint8_t media, + uint8_t codec, struct avdtp_local_sep **lsep, + struct avdtp_remote_sep **rsep); + +int avdtp_unregister_sep(struct avdtp_local_sep *sep); + +const char *avdtp_strerror(struct avdtp_error *err); + +int avdtp_init(void); +void avdtp_exit(void); + +#endif diff --git a/audio/device.c b/audio/device.c index 785d0115..af7cca80 100644 --- a/audio/device.c +++ b/audio/device.c @@ -100,21 +100,24 @@ static DBusMethodVTable device_methods[] = { { NULL, NULL, NULL, NULL } }; -static void device_free(struct device *device) +static void device_free(struct device *dev) { - if (device->headset) - headset_free(device); + if (dev->headset) + headset_free(dev); - if (device->conn) - dbus_connection_unref(device->conn); + if (dev->sink) + sink_free(dev); - if (device->adapter_path) - g_free(device->adapter_path); + if (dev->conn) + dbus_connection_unref(dev->conn); - if (device->path) - g_free(device->path); + if (dev->adapter_path) + g_free(dev->adapter_path); - g_free(device); + if (dev->path) + g_free(dev->path); + + g_free(dev); } static void device_unregister(DBusConnection *conn, void *data) @@ -129,7 +132,7 @@ static void device_unregister(DBusConnection *conn, void *data) struct device *device_register(DBusConnection *conn, const char *path, bdaddr_t *bda) { - struct device *device; + struct device *dev; bdaddr_t src; int dev_id; @@ -141,12 +144,12 @@ struct device *device_register(DBusConnection *conn, if ((dev_id < 0) || (hci_devba(dev_id, &src) < 0)) return NULL; - device = g_new0(struct device, 1); + dev = g_new0(struct device, 1); - if (!dbus_connection_create_object_path(conn, path, device, + if (!dbus_connection_create_object_path(conn, path, dev, device_unregister)) { error("D-Bus failed to register %s path", path); - device_free(device); + device_free(dev); return NULL; } @@ -158,58 +161,69 @@ struct device *device_register(DBusConnection *conn, return NULL; } - device->path = g_strdup(path); - bacpy(&device->dst, bda); - bacpy(&device->src, &src); - device->conn = dbus_connection_ref(conn); - device->adapter_path = g_malloc0(16); - snprintf(device->adapter_path, 16, "/org/bluez/hci%d", dev_id); + dev->path = g_strdup(path); + bacpy(&dev->dst, bda); + bacpy(&dev->src, &src); + dev->conn = dbus_connection_ref(conn); + dev->adapter_path = g_malloc0(16); + snprintf(dev->adapter_path, 16, "/org/bluez/hci%d", dev_id); - return device; + return dev; } -int device_store(struct device *device, gboolean is_default) +int device_store(struct device *dev, gboolean is_default) { char value[64]; char filename[PATH_MAX + 1]; char src_addr[18], dst_addr[18]; + int offset = 0; - if (!device->path) + if (!dev->path) return -EINVAL; - ba2str(&device->dst, dst_addr); - ba2str(&device->src, src_addr); + ba2str(&dev->dst, dst_addr); + ba2str(&dev->src, src_addr); create_name(filename, PATH_MAX, STORAGEDIR, src_addr, "audio"); create_file(filename, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); if (is_default) textfile_put(filename, "default", dst_addr); - if (device->headset) - snprintf(value, 64, "headset"); - else if (device->gateway) - snprintf(value, 64, "gateway"); - else if (device->sink) - snprintf(value, 64, "sink"); - else if (device->source) - snprintf(value, 64, "source"); - else if (device->control) - snprintf(value, 64, "control"); - else - snprintf(value, 64, "target"); + if (dev->headset) { + snprintf(value, 64, "headset "); + offset += strlen("headset "); + } + if (dev->gateway) { + snprintf(value + offset, 64 - offset, "gateway "); + offset += strlen("gateway "); + } + if (dev->sink) { + snprintf(value + offset, 64 - offset, "sink "); + offset += strlen("sink "); + } + if (dev->source) { + snprintf(value + offset, 64 - offset, "source "); + offset += strlen("source "); + } + if (dev->control) { + snprintf(value + offset, 64 - offset, "control "); + offset += strlen("control "); + } + if (dev->target) + snprintf(value + offset, 64 - offset, "target"); return textfile_put(filename, dst_addr, value); } -void device_finish_sdp_transaction(struct device *device) +void device_finish_sdp_transaction(struct device *dev) { char address[18], *addr_ptr = address; DBusMessage *msg, *reply; DBusError derr; - ba2str(&device->dst, address); + ba2str(&dev->dst, address); - msg = dbus_message_new_method_call("org.bluez", device->adapter_path, + msg = dbus_message_new_method_call("org.bluez", dev->adapter_path, "org.bluez.Adapter", "FinishRemoteServiceTransaction"); if (!msg) { @@ -221,7 +235,7 @@ void device_finish_sdp_transaction(struct device *device) DBUS_TYPE_INVALID); dbus_error_init(&derr); - reply = dbus_connection_send_with_reply_and_block(device->conn, + reply = dbus_connection_send_with_reply_and_block(dev->conn, msg, -1, &derr); dbus_message_unref(msg); @@ -236,3 +250,114 @@ void device_finish_sdp_transaction(struct device *device) dbus_message_unref(reply); } + +int device_get_config(struct device *dev, int sock, struct ipc_packet *req, + int pkt_len, struct ipc_data_cfg **rsp) +{ + if (dev->sink && sink_is_active(dev)) + return sink_get_config(dev, sock, req, pkt_len, rsp); + else if (dev->headset && headset_is_active(dev)) + return headset_get_config(dev, sock, req, pkt_len, rsp); + else if (dev->sink) + return sink_get_config(dev, sock, req, pkt_len, rsp); + else if (dev->headset) + return headset_get_config(dev, sock, req, pkt_len, rsp); + + return -EINVAL; +} + +static avdtp_state_t ipc_to_avdtp_state(uint8_t ipc_state) +{ + switch (ipc_state) { + case STATE_DISCONNECTED: + return AVDTP_STATE_IDLE; + case STATE_CONNECTING: + return AVDTP_STATE_CONFIGURED; + case STATE_CONNECTED: + return AVDTP_STATE_OPEN; + case STATE_STREAM_STARTING: + case STATE_STREAMING: + return AVDTP_STATE_STREAMING; + default: + error("Unknown ipc state"); + return AVDTP_STATE_IDLE; + } +} + +static headset_state_t ipc_to_hs_state(uint8_t ipc_state) +{ + switch (ipc_state) { + case STATE_DISCONNECTED: + return HEADSET_STATE_DISCONNECTED; + case STATE_CONNECTING: + return HEADSET_STATE_CONNECT_IN_PROGRESS; + case STATE_CONNECTED: + return HEADSET_STATE_CONNECTED; + case STATE_STREAM_STARTING: + return HEADSET_STATE_PLAY_IN_PROGRESS; + case STATE_STREAMING: + return HEADSET_STATE_PLAYING; + default: + error("Unknown ipc state"); + return HEADSET_STATE_DISCONNECTED; + } +} + +void device_set_state(struct device *dev, uint8_t state) +{ + if (dev->sink && sink_is_active(dev)) + sink_set_state(dev, ipc_to_avdtp_state(state)); + else if (dev->headset && headset_is_active(dev)) + headset_set_state(dev, ipc_to_hs_state(state)); +} + +static uint8_t avdtp_to_ipc_state(avdtp_state_t state) +{ + switch (state) { + case AVDTP_STATE_IDLE: + return STATE_DISCONNECTED; + case AVDTP_STATE_CONFIGURED: + return STATE_CONNECTING; + case AVDTP_STATE_OPEN: + return STATE_CONNECTED; + case AVDTP_STATE_STREAMING: + return STATE_STREAMING; + default: + error("Unknown avdt state"); + return AVDTP_STATE_IDLE; + } +} + +static uint8_t hs_to_ipc_state(headset_state_t state) +{ + switch (state) { + case HEADSET_STATE_DISCONNECTED: + return STATE_DISCONNECTED; + case HEADSET_STATE_CONNECT_IN_PROGRESS: + return STATE_CONNECTING; + case HEADSET_STATE_CONNECTED: + return STATE_CONNECTED; + case HEADSET_STATE_PLAY_IN_PROGRESS: + return STATE_STREAMING; + default: + error("Unknown headset state"); + return AVDTP_STATE_IDLE; + } +} + +uint8_t device_get_state(struct device *dev) +{ + avdtp_state_t sink_state; + headset_state_t hs_state; + + if (dev->sink && sink_is_active(dev)) { + sink_state = sink_get_state(dev); + return avdtp_to_ipc_state(sink_state); + } + else if (dev->headset && headset_is_active(dev)) { + hs_state = headset_get_state(dev); + return hs_to_ipc_state(hs_state); + } + + return STATE_DISCONNECTED; +} diff --git a/audio/device.h b/audio/device.h index 71e1053f..62c13e9e 100644 --- a/audio/device.h +++ b/audio/device.h @@ -75,3 +75,10 @@ struct device *device_register(DBusConnection *conn, int device_store(struct device *device, gboolean is_default); void device_finish_sdp_transaction(struct device *device); + +int device_get_config(struct device *dev, int sock, struct ipc_packet *req, + int pkt_len, struct ipc_data_cfg **rsp); + +void device_set_state(struct device *dev, uint8_t state); + +uint8_t device_get_state(struct device *dev); diff --git a/audio/headset.c b/audio/headset.c index 9dccc5ad..2bc8583b 100644 --- a/audio/headset.c +++ b/audio/headset.c @@ -53,6 +53,7 @@ #include "logging.h" #include "manager.h" #include "error.h" +#include "unix.h" #define RING_INTERVAL 3000 @@ -68,6 +69,7 @@ struct pending_connect { DBusMessage *msg; GIOChannel *io; struct ipc_packet *pkt; + int pkt_len; guint io_id; int sock; int err; @@ -102,7 +104,7 @@ static int rfcomm_connect(struct device *device, struct pending_connect *c); static void pending_connect_free(struct pending_connect *c) { if (c->pkt) - unix_send_cfg(c->sock, c->pkt); + g_free(c->pkt); if (c->io) { g_io_channel_close(c->io); g_io_channel_unref(c->io); @@ -339,8 +341,19 @@ static void pending_connect_ok(struct pending_connect *c, struct device *dev) if (reply) send_message_and_unref(dev->conn, reply); } - else if (c->pkt) - headset_get_config(dev, c->sock, c->pkt); + else if (c->pkt) { + struct ipc_data_cfg *rsp; + int ret; + + ret = headset_get_config(dev, c->sock, c->pkt, + c->pkt_len, &rsp); + if (ret == 0) { + unix_send_cfg(c->sock, rsp); + g_free(rsp); + } + else + unix_send_cfg(c->sock, NULL); + } pending_connect_free(c); } @@ -349,6 +362,8 @@ static void pending_connect_failed(struct pending_connect *c, struct device *dev { if (c->msg) err_connect_failed(dev->conn, c->msg, strerror(c->err)); + if (c->pkt) + unix_send_cfg(c->sock, NULL); pending_connect_free(c); } @@ -1358,7 +1373,8 @@ register_iface: void headset_free(void *device) { - struct headset *hs = ((struct device *) device)->headset; + struct device *dev = device; + struct headset *hs = dev->headset; if (hs->sco) { g_io_channel_close(hs->sco); @@ -1371,54 +1387,54 @@ void headset_free(void *device) } g_free(hs); - hs = NULL; + dev->headset = NULL; } -int headset_get_config(void *device, int sock, struct ipc_packet *pkt) +int headset_get_config(void *device, int sock, struct ipc_packet *pkt, + int pkt_len, struct ipc_data_cfg **cfg) { struct headset *hs = ((struct device *) device)->headset; - struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data; int err = EINVAL; struct pending_connect *c; + struct ipc_data_cfg *rsp; + + if (hs->rfcomm && hs->sco) + goto proceed; - if (hs->rfcomm == NULL) { - c = g_try_new0(struct pending_connect, 1); - if (c == NULL) - goto error; - c->sock = sock; - c->pkt = pkt; + c = g_new0(struct pending_connect, 1); + c->sock = sock; + c->pkt = g_malloc(pkt_len); + memcpy(c->pkt, pkt, pkt_len); + + if (hs->rfcomm == NULL) err = rfcomm_connect(device, c); - if (err < 0) - goto error; - return 0; - } - else if (hs->sco == NULL) { - c = g_try_new0(struct pending_connect, 1); - if (c == NULL) - goto error; - c->sock = sock; - c->pkt = pkt; + else if (hs->sco == NULL) err = sco_connect(device, c); - if (err < 0) - goto error; - return 0; - } + else + goto error; + + if (err < 0) + goto error; - cfg->fd = g_io_channel_unix_get_fd(hs->sco); - cfg->fd_opt = CFG_FD_OPT_READWRITE; - cfg->encoding = 0; - cfg->bitpool = 0; - cfg->channels = 1; - cfg->pkt_len = 48; - cfg->sample_size = 2; - cfg->rate = 8000; + return 1; + +proceed: + *cfg = g_new0(struct ipc_data_cfg, 1); + rsp = *cfg; + rsp->fd = g_io_channel_unix_get_fd(hs->sco); + rsp->fd_opt = CFG_FD_OPT_READWRITE; + rsp->codec = CFG_CODEC_NONE; + rsp->channels = 1; + rsp->channel_mode = CFG_CHANNEL_MODE_MONO; + rsp->pkt_len = 48; + rsp->sample_size = 2; + rsp->rate = 8000; return 0; error: if (c) pending_connect_free(c); - cfg->fd = -1; return -err; } @@ -1547,3 +1563,14 @@ int headset_get_channel(void *device) return hs->rfcomm_ch; } + +gboolean headset_is_active(void *device) +{ + struct device *dev = device; + struct headset *hs = dev->headset; + + if (hs->state != HEADSET_STATE_DISCONNECTED) + return TRUE; + + return FALSE; +} diff --git a/audio/headset.h b/audio/headset.h index d3fd86d9..530bdea8 100644 --- a/audio/headset.h +++ b/audio/headset.h @@ -20,12 +20,15 @@ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * */ +#ifndef __AUDIO_HEADSET_H__ +#define __AUDIO_HEADSET_H__ + #include <bluetooth/sdp.h> #include <bluetooth/sdp_lib.h> #include <dbus/dbus.h> -#include "unix.h" +#include "ipc.h" #define AUDIO_HEADSET_INTERFACE "org.bluez.audio.Headset" @@ -40,11 +43,11 @@ typedef enum { } headset_event_t; typedef enum { - HEADSET_STATE_DISCONNECTED = STATE_DISCONNECTED, - HEADSET_STATE_CONNECT_IN_PROGRESS = STATE_CONNECTING, - HEADSET_STATE_CONNECTED = STATE_CONNECTED, - HEADSET_STATE_PLAY_IN_PROGRESS = STATE_STREAM_STARTING, - HEADSET_STATE_PLAYING = STATE_STREAMING, + HEADSET_STATE_DISCONNECTED, + HEADSET_STATE_CONNECT_IN_PROGRESS, + HEADSET_STATE_CONNECTED, + HEADSET_STATE_PLAY_IN_PROGRESS, + HEADSET_STATE_PLAYING } headset_state_t; typedef enum { @@ -55,13 +58,14 @@ typedef enum { struct headset; struct headset *headset_init(void *device, sdp_record_t *record, - uint16_t svc); + uint16_t svc); void headset_free(void *device); void headset_update(void *device, sdp_record_t *record, uint16_t svc); -int headset_get_config(void *device, int sock, struct ipc_packet *pkt); +int headset_get_config(void *device, int sock, struct ipc_packet *pkt, + int pkt_len, struct ipc_data_cfg **rsp); headset_type_t headset_get_type(void *device); void headset_set_type(void *device, headset_type_t type); @@ -73,3 +77,7 @@ headset_state_t headset_get_state(void *device); void headset_set_state(void *device, headset_state_t state); int headset_get_channel(void *device); + +gboolean headset_is_active(void *device); + +#endif diff --git a/audio/ipc.h b/audio/ipc.h index e56dca24..bd31abbc 100644 --- a/audio/ipc.h +++ b/audio/ipc.h @@ -20,11 +20,15 @@ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * */ +#ifndef __AUDIO_IPC_H__ +#define __AUDIO_IPC_H__ #include <stdint.h> #define IPC_TYPE_CONNECT 0x0001 +#define IPC_MTU 32 + #define IPC_SOCKET_NAME "\0/org/bluez/audio" #ifndef UNIX_PATH_MAX @@ -32,22 +36,22 @@ #endif /* Supported roles */ -#define PKT_ROLE_NONE 0 -#define PKT_ROLE_AUTO 1 -#define PKT_ROLE_VOICE 2 -#define PKT_ROLE_HIFI 3 +#define PKT_ROLE_NONE 0 +#define PKT_ROLE_AUTO 1 +#define PKT_ROLE_VOICE 2 +#define PKT_ROLE_HIFI 3 /* Packet types */ -#define PKT_TYPE_CFG_REQ 0 -#define PKT_TYPE_CFG_RSP 1 -#define PKT_TYPE_STATE_REQ 2 -#define PKT_TYPE_STATE_RSP 3 -#define PKT_TYPE_CTL_REQ 4 -#define PKT_TYPE_CTL_RSP 5 -#define PKT_TYPE_CTL_NTFY 6 +#define PKT_TYPE_CFG_REQ 0 +#define PKT_TYPE_CFG_RSP 1 +#define PKT_TYPE_STATE_REQ 2 +#define PKT_TYPE_STATE_RSP 3 +#define PKT_TYPE_CTL_REQ 4 +#define PKT_TYPE_CTL_RSP 5 +#define PKT_TYPE_CTL_NTFY 6 /* Errors codes */ -#define PKT_ERROR_NONE 0 +#define PKT_ERROR_NONE 0 struct ipc_packet { uint8_t id; /* Device id */ @@ -59,52 +63,76 @@ struct ipc_packet { } __attribute__ ((packed)); /* File descriptor options */ -#define CFG_FD_OPT_READ 0 -#define CFG_FD_OPT_WRITE 1 -#define CFG_FD_OPT_READWRITE 2 +#define CFG_FD_OPT_READ 0 +#define CFG_FD_OPT_WRITE 1 +#define CFG_FD_OPT_READWRITE 2 + +/* Audio channel mode */ +#define CFG_CHANNEL_MODE_MONO (1 << 3) +#define CFG_CHANNEL_MODE_DUAL_CHANNEL (1 << 2) +#define CFG_CHANNEL_MODE_STEREO (1 << 1) +#define CFG_CHANNEL_MODE_JOINT_STEREO 1 + +/* Codec options */ +#define CFG_CODEC_NONE 0 +#define CFG_CODEC_SBC 1 struct ipc_data_cfg { int fd; /* Stream file descriptor */ uint8_t fd_opt; /* Stream file descriptor options: read, write or readwrite*/ - uint8_t encoding; /* Stream encoding */ - uint8_t bitpool; /* Encoding bitpool */ uint8_t channels; /* Number of audio channel */ - uint8_t pkt_len; /* Stream packet length */ + uint8_t channel_mode; /* Audio channel mode*/ + uint16_t pkt_len; /* Stream packet length */ uint8_t sample_size; /* Sample size in bytes */ uint16_t rate; /* Stream sample rate */ + uint8_t codec; /* Stream codec */ + uint8_t data[0]; /* Codec payload */ +} __attribute__ ((packed)); + +/* SBC codec options */ +#define CODEC_SBC_ALLOCATION_SNR (1 << 1) +#define CODEC_SBC_ALLOCATION_LOUDNESS 1 + +struct ipc_codec_sbc { + uint8_t allocation; + uint8_t subbands; + uint8_t blocks; + uint8_t bitpool; } __attribute__ ((packed)); /* Device status */ -#define STATE_DISCONNECTED 0 -#define STATE_CONNECTING 1 -#define STATE_CONNECTED 2 -#define STATE_STREAM_STARTING 3 -#define STATE_STREAMING 4 +#define STATE_DISCONNECTED 0 +#define STATE_CONNECTING 1 +#define STATE_CONNECTED 2 +#define STATE_STREAM_STARTING 3 +#define STATE_STREAMING 4 struct ipc_data_state { uint8_t state; /* Stream state */ } __attribute__ ((packed)); -#define CTL_MODE_PLAYBACK 0 -#define CTL_MODE_CAPTURE 1 -#define CTL_MODE_GENERAL 2 +#define CTL_MODE_PLAYBACK 0 +#define CTL_MODE_CAPTURE 1 +#define CTL_MODE_GENERAL 2 /* Supported control operations */ -#define CTL_KEY_POWER 0x40 -#define CTL_KEY_VOL_UP 0x41 -#define CTL_KEY_VOL_DOWN 0x42 -#define CTL_KEY_MUTE 0x43 -#define CTL_KEY_PLAY 0x44 -#define CTL_KEY_STOP 0x45 -#define CTL_KEY_PAUSE 0x46 -#define CTL_KEY_RECORD 0x47 -#define CTL_KEY_REWIND 0x48 -#define CTL_KEY_FAST_FORWARD 0x49 -#define CTL_KEY_EJECT 0x4A -#define CTL_KEY_FORWARD 0x4B -#define CTL_KEY_BACKWARD 0x4C +#define CTL_KEY_POWER 0x40 +#define CTL_KEY_VOL_UP 0x41 +#define CTL_KEY_VOL_DOWN 0x42 +#define CTL_KEY_MUTE 0x43 +#define CTL_KEY_PLAY 0x44 +#define CTL_KEY_STOP 0x45 +#define CTL_KEY_PAUSE 0x46 +#define CTL_KEY_RECORD 0x47 +#define CTL_KEY_REWIND 0x48 +#define CTL_KEY_FAST_FORWARD 0x49 +#define CTL_KEY_EJECT 0x4A +#define CTL_KEY_FORWARD 0x4B +#define CTL_KEY_BACKWARD 0x4C struct ipc_data_ctl { uint8_t mode; /* Control Mode */ uint8_t key; /* Control Key */ } __attribute__ ((packed)); + +#endif diff --git a/audio/main.c b/audio/main.c index 4da85a29..72e54133 100644 --- a/audio/main.c +++ b/audio/main.c @@ -35,7 +35,7 @@ #include "dbus.h" #include "logging.h" - +#include "unix.h" #include "manager.h" static gboolean disable_hfp = TRUE; @@ -43,6 +43,15 @@ static gboolean sco_hci = FALSE; static GMainLoop *main_loop = NULL; +static struct enabled_interfaces enabled = { + .headset = TRUE, + .gateway = FALSE, + .sink = TRUE, + .source = FALSE, + .control = FALSE, + .target = FALSE, +}; + static void sig_term(int sig) { g_main_loop_quit(main_loop); @@ -53,7 +62,7 @@ static void read_config(const char *file) GKeyFile *keyfile; GError *err = NULL; gboolean no_hfp; - char *sco_routing; + char *str; keyfile = g_key_file_new(); @@ -64,21 +73,42 @@ static void read_config(const char *file) return; } - sco_routing = g_key_file_get_string(keyfile, "General", + str = g_key_file_get_string(keyfile, "General", "SCORouting", &err); if (err) { debug("%s: %s", file, err->message); g_error_free(err); err = NULL; } else { - if (strcmp(sco_routing, "PCM") == 0) + if (strcmp(str, "PCM") == 0) sco_hci = FALSE; - else if (strcmp(sco_routing, "HCI") == 0) + else if (strcmp(str, "HCI") == 0) sco_hci = TRUE; else - error("Invalid Headset Routing value: %s", - sco_routing); - g_free(sco_routing); + error("Invalid Headset Routing value: %s", str); + g_free(str); + } + + str = g_key_file_get_string(keyfile, "General", + "Disabled", &err); + if (err) { + debug("%s: %s", file, err->message); + g_error_free(err); + err = NULL; + } else { + if (strstr(str, "Headset")) + enabled.headset = FALSE; + if (strstr(str, "Gateway")) + enabled.gateway = FALSE; + if (strstr(str, "Sink")) + enabled.sink = FALSE; + if (strstr(str, "Source")) + enabled.source = FALSE; + if (strstr(str, "Control")) + enabled.control = FALSE; + if (strstr(str, "Target")) + enabled.target = FALSE; + g_free(str); } no_hfp = g_key_file_get_boolean(keyfile, "Headset", @@ -131,7 +161,7 @@ int main(int argc, char *argv[]) exit(1); } - if (audio_init(conn, disable_hfp, sco_hci) < 0) { + if (audio_init(conn, &enabled, disable_hfp, sco_hci) < 0) { error("Audio init failed!"); exit(1); } diff --git a/audio/manager.c b/audio/manager.c index 7e250785..58f19ed0 100644 --- a/audio/manager.c +++ b/audio/manager.c @@ -52,6 +52,8 @@ #include "textfile.h" #include "manager.h" #include "error.h" +#include "a2dp.h" +#include "avdtp.h" typedef enum { HEADSET = 1 << 0, @@ -93,6 +95,8 @@ static uint32_t hf_record_id = 0; static GIOChannel *hs_server = NULL; static GIOChannel *hf_server = NULL; +static const struct enabled_interfaces *enabled; + static void get_next_record(struct audio_sdp_data *data); static DBusHandlerResult get_handles(const char *uuid, struct audio_sdp_data *data); @@ -190,6 +194,8 @@ static gboolean server_is_enabled(uint16_t svc) case HANDSFREE_SVCLASS_ID: ret = (hf_server != NULL); break; + case AUDIO_SINK_SVCLASS_ID: + return enabled->sink; default: ret = FALSE; break; @@ -233,6 +239,8 @@ static void handle_record(sdp_record_t *record, struct device *device) break; case AUDIO_SINK_SVCLASS_ID: debug("Found Audio Sink"); + if (device->sink == NULL) + device->sink = sink_init(device); break; case AUDIO_SOURCE_SVCLASS_ID: debug("Found Audio Source"); @@ -713,7 +721,7 @@ static gboolean device_supports_interface(struct device *device, return device->source ? TRUE : FALSE; if (strcmp(iface, AUDIO_SINK_INTERFACE) == 0) - return device->sink ? TRUE : FALSE; + return device->sink ? TRUE : FALSE; if (strcmp(iface, AUDIO_CONTROL_INTERFACE) == 0) return device->control ? TRUE : FALSE; @@ -1135,9 +1143,10 @@ static void parse_stored_devices(char *key, char *value, void *data) if (!device) return; - if (strncmp(value, "headset", strlen("headset")) == 0) + if (strstr(value, "headset")) device->headset = headset_init(device, NULL, 0); - + if (strstr(value, "sink")) + device->sink = sink_init(device); add_device(device); } @@ -1353,7 +1362,7 @@ static int hfp_ag_record(sdp_buf_t *buf, uint8_t ch) return ret; } -static uint32_t add_record(uint8_t channel, sdp_buf_t *buf) +uint32_t add_service_record(DBusConnection *conn, sdp_buf_t *buf) { DBusMessage *msg, *reply; DBusError derr; @@ -1397,12 +1406,12 @@ static uint32_t add_record(uint8_t channel, sdp_buf_t *buf) dbus_message_unref(reply); - debug("add_record: got record id 0x%x", rec_id); + debug("add_service_record: got record id 0x%x", rec_id); return rec_id; } -static int remove_record(uint32_t rec_id) +int remove_service_record(DBusConnection *conn, uint32_t rec_id) { DBusMessage *msg, *reply; DBusError derr; @@ -1633,11 +1642,14 @@ static GIOChannel *server_socket(uint8_t *channel) return io; } -static int server_init(DBusConnection *conn, gboolean no_hfp) +static int headset_server_init(DBusConnection *conn, gboolean no_hfp) { uint8_t chan = DEFAULT_HS_AG_CHANNEL; sdp_buf_t buf; + if (!(enabled->headset || enabled->gateway)) + return 0; + hs_server = server_socket(&chan); if (!hs_server) return -1; @@ -1647,7 +1659,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp) return -1; } - hs_record_id = add_record(chan, &buf); + hs_record_id = add_service_record(conn, &buf); free(buf.data); if (!hs_record_id) { error("Unable to register HS AG service record"); @@ -1673,7 +1685,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp) return -1; } - hf_record_id = add_record(chan, &buf); + hf_record_id = add_service_record(conn, &buf); free(buf.data); if (!hf_record_id) { error("Unable to register HS AG service record"); @@ -1691,7 +1703,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp) static void server_exit(void) { if (hs_record_id) { - remove_record(hs_record_id); + remove_service_record(connection, hs_record_id); hs_record_id = 0; } @@ -1701,7 +1713,7 @@ static void server_exit(void) } if (hf_record_id) { - remove_record(hf_record_id); + remove_service_record(connection, hf_record_id); hf_record_id = 0; } @@ -1711,11 +1723,17 @@ static void server_exit(void) } } -int audio_init(DBusConnection *conn, gboolean no_hfp, gboolean sco_hci) +int audio_init(DBusConnection *conn, struct enabled_interfaces *enable, + gboolean no_hfp, gboolean sco_hci) { connection = dbus_connection_ref(conn); - if (server_init(conn, no_hfp) < 0) + enabled = enable; + + if (headset_server_init(conn, no_hfp) < 0) + goto failed; + + if (a2dp_init(conn, enable->sink, enable->source) < 0) goto failed; if (!dbus_connection_create_object_path(conn, AUDIO_MANAGER_PATH, @@ -1754,7 +1772,24 @@ void audio_exit(void) connection = NULL; } -struct device *manager_default_device() +struct device *manager_default_device(void) { return default_dev; } + +struct device *manager_get_connected_device(void) +{ + GSList *l; + + for (l = devices; l != NULL; l = g_slist_next(l)) { + struct device *device = l->data; + + if (device->sink && sink_is_active(device)) + return device; + + if (device->headset && headset_is_active(device)) + return device; + } + + return NULL; +} diff --git a/audio/manager.h b/audio/manager.h index 79fe9090..9fbc4940 100644 --- a/audio/manager.h +++ b/audio/manager.h @@ -29,10 +29,25 @@ #define AUDIO_MANAGER_PATH "/org/bluez/audio" #define AUDIO_MANAGER_INTERFACE "org.bluez.audio.Manager" -int audio_init(DBusConnection *conn, gboolean no_hfp, gboolean sco_hci); +struct enabled_interfaces { + gboolean headset; + gboolean gateway; + gboolean sink; + gboolean source; + gboolean control; + gboolean target; +}; + +int audio_init(DBusConnection *conn, struct enabled_interfaces *enabled, + gboolean no_hfp, gboolean sco_hci); void audio_exit(void); +uint32_t add_service_record(DBusConnection *conn, sdp_buf_t *buf); +int remove_service_record(DBusConnection *conn, uint32_t rec_id); + struct device *manager_device_connected(bdaddr_t *bda); struct device *manager_default_device(); + +struct device *manager_get_connected_device(void); diff --git a/audio/pcm_bluetooth.c b/audio/pcm_bluetooth.c index 92d0383c..3f428ecd 100644 --- a/audio/pcm_bluetooth.c +++ b/audio/pcm_bluetooth.c @@ -27,6 +27,9 @@ #include <sys/socket.h> #include <sys/un.h> +#include <sys/time.h> + +#include <netinet/in.h> #include <alsa/asoundlib.h> #include <alsa/pcm_external.h> @@ -35,6 +38,11 @@ #include <bluetooth/sco.h> #include "ipc.h" +#include "sbc.h" + +/*#define ENABLE_DEBUG */ + +#define BUFFER_SIZE 1024 #ifdef ENABLE_DEBUG #define DBG(fmt, arg...) printf("DEBUG: %s: " fmt "\n" , __FUNCTION__ , ## arg) @@ -50,15 +58,65 @@ #define SCO_RXBUFS 0x04 #endif +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)); + +struct bluetooth_a2dp { + sbc_t sbc; /* Codec data */ + int samples; /* Number of encoded samples */ + time_t timestamp; /* Codec samples timestamp */ + uint8_t buffer[BUFFER_SIZE]; /* Codec transfer buffer */ + int count; /* Codec transfer buffer counter */ + + int nsamples; /* Cumulative number of codec samples */ + struct timeval ntimestamp; /* Cumulative timeval */ + uint16_t seq_num; /* */ + int frame_count; /* */ + + int bandwithcount; + struct timeval bandwithtimestamp; +}; + struct bluetooth_data { snd_pcm_ioplug_t io; snd_pcm_sframes_t hw_ptr; struct ipc_data_cfg cfg; /* Bluetooth device config */ int sock; /* Daemon unix socket */ - uint8_t *buffer; /* Transfer buffer */ - uint8_t count; /* Transfer buffer counter */ + uint8_t buffer[BUFFER_SIZE]; /* Encoded transfer buffer */ + int count; /* Transfer buffer counter */ + struct bluetooth_a2dp a2dp; /* a2dp data */ }; +void memcpy_changeendian(void *dst, const void *src, int size) +{ + int i; + const uint16_t *ptrsrc = src; + uint16_t *ptrdst = dst; + for (i = 0; i < size / 2; i++) { + *ptrdst++ = htons(*ptrsrc++); + } +} + static int bluetooth_start(snd_pcm_ioplug_t *io) { DBG("bluetooth_start %p", io); @@ -77,49 +135,21 @@ static snd_pcm_sframes_t bluetooth_pointer(snd_pcm_ioplug_t *io) { struct bluetooth_data *data = io->private_data; - DBG("bluetooth_pointer %p", io); - - DBG("hw_ptr=%lu", data->hw_ptr); +#if 0 + DBG("bluetooth_pointer %p, hw_ptr=%lu", io, data->hw_ptr); +#endif return data->hw_ptr; } static void bluetooth_exit(struct bluetooth_data *data) { - int ret, len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_state); - struct ipc_packet *pkt; - struct ipc_data_state *state; - - DBG("Sending PKT_TYPE_STATUS_REQ..."); - - if ((pkt = malloc(len)) == NULL) - goto done; - - memset(pkt, 0, len); - pkt->type = PKT_TYPE_STATE_REQ; - pkt->role = PKT_ROLE_NONE; - pkt->error = PKT_ERROR_NONE; - - state = (struct ipc_data_state *) pkt->data; - state->state = STATE_DISCONNECTED; - - if ((ret = send(data->sock, pkt, len, 0)) < 0) - DBG("Error %s (%d)", strerror(errno), errno); - - free(pkt); -done: - if (data == NULL) - return; - if (data->sock >= 0) close(data->sock); if (data->cfg.fd >= 0) close(data->cfg.fd); - if (data->buffer) - free(data->buffer); - free(data); } @@ -127,7 +157,7 @@ static int bluetooth_close(snd_pcm_ioplug_t *io) { struct bluetooth_data *data = io->private_data; - DBG("bluetooth_close %p", io); + DBG("%p", io); bluetooth_exit(data); @@ -170,7 +200,7 @@ static int bluetooth_hw_params(snd_pcm_ioplug_t *io, snd_pcm_hw_params_t *params return 0; opt_name = (io->stream == SND_PCM_STREAM_PLAYBACK) ? - SO_SNDBUF : SO_RCVBUF; + SO_SNDBUF : SO_RCVBUF; if (setsockopt(cfg.fd, SOL_SCO, opt_name, &period_count, sizeof(period_count)) == 0) @@ -178,14 +208,15 @@ static int bluetooth_hw_params(snd_pcm_ioplug_t *io, snd_pcm_hw_params_t *params err = errno; SNDERR("%s (%d)", strerror(err), err); + bluetooth_close(io); return -err; } -static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io, - const snd_pcm_channel_area_t *areas, - snd_pcm_uframes_t offset, - snd_pcm_uframes_t size) +static snd_pcm_sframes_t bluetooth_hsp_read(snd_pcm_ioplug_t *io, + const snd_pcm_channel_area_t *areas, + snd_pcm_uframes_t offset, + snd_pcm_uframes_t size) { struct bluetooth_data *data = io->private_data; struct ipc_data_cfg cfg = data->cfg; @@ -193,8 +224,9 @@ static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io, unsigned char *buff; int nrecv, frame_size = 0; - DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu, io->nonblock=%u", - areas->step, areas->first, offset, size, io->nonblock); + DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu," + "io->nonblock=%u", areas->step, areas->first, offset, size, + io->nonblock); if (data->count > 0) goto proceed; @@ -216,10 +248,12 @@ static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io, } /* Increment hardware transmition pointer */ - data->hw_ptr = (data->hw_ptr + cfg.pkt_len / cfg.sample_size) % io->buffer_size; + data->hw_ptr = (data->hw_ptr + cfg.pkt_len / cfg.sample_size) % + io->buffer_size; proceed: - buff = (unsigned char *) areas->addr + (areas->first + areas->step * offset) / 8; + buff = (unsigned char *) areas->addr + + (areas->first + areas->step * offset) / 8; if ((data->count + size * frame_size) <= cfg.pkt_len) frames_to_write = size; @@ -238,10 +272,10 @@ done: return ret; } -static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io, - const snd_pcm_channel_area_t *areas, - snd_pcm_uframes_t offset, - snd_pcm_uframes_t size) +static snd_pcm_sframes_t bluetooth_hsp_write(snd_pcm_ioplug_t *io, + const snd_pcm_channel_area_t *areas, + snd_pcm_uframes_t offset, + snd_pcm_uframes_t size) { struct bluetooth_data *data = io->private_data; struct ipc_data_cfg cfg = data->cfg; @@ -263,10 +297,11 @@ static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io, DBG("count = %d, frames_to_read = %lu", data->count, frames_to_read); /* Ready for more data */ - buff = (uint8_t *) areas->addr + (areas->first + areas->step * offset) / 8; + buff = (uint8_t *) areas->addr + + (areas->first + areas->step * offset) / 8; memcpy(data->buffer + data->count, buff, frame_size * frames_to_read); - /* Remember we have some frame in the pipe now */ + /* Remember we have some frames in the pipe now */ data->count += frames_to_read * frame_size; if (data->count != cfg.pkt_len) { ret = frames_to_read; @@ -290,28 +325,245 @@ static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io, ret = -EIO; done: - DBG("returning %d", (int)ret); + DBG("returning %lu", ret); + return ret; +} + +static snd_pcm_sframes_t bluetooth_a2dp_read(snd_pcm_ioplug_t *io, + const snd_pcm_channel_area_t *areas, + snd_pcm_uframes_t offset, + snd_pcm_uframes_t size) +{ + snd_pcm_uframes_t ret = 0; + return ret; +} + +static int avdtp_write(struct bluetooth_a2dp *a2dp, struct ipc_data_cfg *cfg, + unsigned int nonblock) +{ + int count = 0; + int written; + struct rtp_header *header; + struct rtp_payload *payload; +#ifdef ENABLE_DEBUG + static struct timeval send_date = { 0, 0 }; + static struct timeval prev_date = { 0, 0 }; + struct timeval send_delay = { 0, 0 }; + struct timeval sendz_delay = { 0, 0 }; +#endif + + header = (void *) a2dp->buffer; + payload = (void *) (a2dp->buffer + sizeof(*header)); + + 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); + + while (count++ < 50) { +#ifdef ENABLE_DEBUG + gettimeofday(&send_date, NULL); +#endif + written = send(cfg->fd, a2dp->buffer, a2dp->count, + nonblock ? MSG_DONTWAIT : 0); + +#ifdef ENABLE_DEBUG + if ((written >= 0 || errno == EAGAIN) && prev_date.tv_sec != 0) { + long delay, real, theo, delta; + + delay = (long) (send_delay.tv_sec * 1000 + + send_delay.tv_usec / 1000), + real = (long) (sendz_delay.tv_sec * 1000 + + sendz_delay.tv_usec / 1000); + theo = (long) (((float) a2dp->nsamples) / + cfg->rate * 1000.0); + delta = (long) (sendz_delay.tv_sec * 1000 + + sendz_delay.tv_usec / 1000) - + (long) (((float) a2dp->nsamples) / + cfg->rate * 1000.0); + + timersub(&send_date, &prev_date, &send_delay); + timersub(&send_date, &a2dp->ntimestamp, &sendz_delay); + + DBG("send %d (cumul=%d) samples (delay=%ld ms," + " real=%ld ms, theo=%ld ms," + " delta=%ld ms).", a2dp->samples, + a2dp->nsamples, delay, real, theo, + delta); + } +#endif + if (written >= 0) + break; + + if (errno != EAGAIN) + break; + + DBG("send (retry)."); + usleep(150000); + } + +#ifdef ENABLE_DEBUG + prev_date = send_date; +#endif + + /* Send our data */ + if (written != a2dp->count) + DBG("Wrote %d not %d bytes", written, a2dp->count); +#if 0 + else { + /* Measure bandwith usage */ + struct timeval now = { 0, 0 }; + struct timeval interval = { 0, 0 }; + + if(a2dp->bandwithtimestamp.tv_sec == 0) + gettimeofday(&a2dp->bandwithtimestamp, NULL); + + /* See if we must wait again */ + gettimeofday(&now, NULL); + timersub(&now, &a2dp->bandwithtimestamp, &interval); + if(interval.tv_sec > 0) + DBG("Bandwith: %d (%d kbps)", a2dp->bandwithcount, + a2dp->bandwithcount/128); + a2dp->bandwithtimestamp = now; + a2dp->bandwithcount = 0; + } + + a2dp->bandwithcount += written; + +#endif + /* 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 written; +} + +static snd_pcm_sframes_t bluetooth_a2dp_write(snd_pcm_ioplug_t *io, + const snd_pcm_channel_area_t *areas, + snd_pcm_uframes_t offset, + snd_pcm_uframes_t size) +{ + struct bluetooth_data *data = io->private_data; + struct bluetooth_a2dp *a2dp = &data->a2dp; + snd_pcm_sframes_t ret = 0; + snd_pcm_uframes_t frames_to_read; + int frame_size, encoded; + uint8_t *buff; + static int codesize = 0; + + DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu," + "io->nonblock=%u", areas->step, areas->first, + offset, size, io->nonblock); + + if (codesize == 0) { + /* How much data can be encoded by sbc at a time? */ + codesize = a2dp->sbc.subbands * a2dp->sbc.blocks * + a2dp->sbc.channels * 2; + /* Reserv header space in outgoing buffer */ + a2dp->count = sizeof(struct rtp_header) + + sizeof(struct rtp_payload); + gettimeofday(&a2dp->ntimestamp, NULL); + } + + frame_size = areas->step / 8; + if ((data->count + size * frame_size) <= codesize) + frames_to_read = size; + else + frames_to_read = (codesize - data->count) / frame_size; + + DBG("count = %d, frames_to_read = %lu", data->count, frames_to_read); + DBG("a2dp.count = %d cfg.pkt_len = %d", a2dp->count, + data->cfg.pkt_len); + + /* FIXME: If state is not streaming then return */ + + /* Ready for more data */ + buff = (uint8_t *) areas->addr + + (areas->first + areas->step * offset) / 8; + memcpy_changeendian(data->buffer + data->count, buff, + frame_size * frames_to_read); + + /* Remember we have some frames in the pipe now */ + data->count += frames_to_read * frame_size; + if (data->count != codesize) { + ret = frames_to_read; + goto done; + } + + /* Enough data to encode (sbc wants 1k blocks) */ + encoded = sbc_encode(&(a2dp->sbc), data->buffer, codesize); + if (encoded <= 0) { + DBG("Encoding error %d", encoded); + goto done; + } + + data->count -= encoded; + + DBG("encoded = %d a2dp.sbc.len= %d", encoded, a2dp->sbc.len); + + if (a2dp->count + a2dp->sbc.len >= data->cfg.pkt_len) + avdtp_write(a2dp, &data->cfg, io->nonblock); + + memcpy(a2dp->buffer + a2dp->count, a2dp->sbc.data, a2dp->sbc.len); + a2dp->count += a2dp->sbc.len; + a2dp->frame_count++; + a2dp->samples += encoded / frame_size; + a2dp->nsamples += encoded / frame_size; + /* Increment hardware transmition pointer */ + data->hw_ptr = (data->hw_ptr + codesize / frame_size) + % io->buffer_size; + + ret = frames_to_read; + +done: + DBG("returning %lu", ret); return ret; } -static snd_pcm_ioplug_callback_t bluetooth_playback_callback = { +static snd_pcm_ioplug_callback_t bluetooth_hsp_playback = { + .start = bluetooth_start, + .stop = bluetooth_stop, + .pointer = bluetooth_pointer, + .close = bluetooth_close, + .hw_params = bluetooth_hw_params, + .prepare = bluetooth_prepare, + .transfer = bluetooth_hsp_write, +}; + +static snd_pcm_ioplug_callback_t bluetooth_hsp_capture = { .start = bluetooth_start, .stop = bluetooth_stop, .pointer = bluetooth_pointer, .close = bluetooth_close, .hw_params = bluetooth_hw_params, .prepare = bluetooth_prepare, - .transfer = bluetooth_write, + .transfer = bluetooth_hsp_read, }; -static snd_pcm_ioplug_callback_t bluetooth_capture_callback = { +static snd_pcm_ioplug_callback_t bluetooth_a2dp_playback = { .start = bluetooth_start, .stop = bluetooth_stop, .pointer = bluetooth_pointer, .close = bluetooth_close, .hw_params = bluetooth_hw_params, .prepare = bluetooth_prepare, - .transfer = bluetooth_read, + .transfer = bluetooth_a2dp_write, +}; + +static snd_pcm_ioplug_callback_t bluetooth_a2dp_capture = { + .start = bluetooth_start, + .stop = bluetooth_stop, + .pointer = bluetooth_pointer, + .close = bluetooth_close, + .hw_params = bluetooth_hw_params, + .prepare = bluetooth_prepare, + .transfer = bluetooth_a2dp_read, }; #define ARRAY_NELEMS(a) (sizeof((a)) / sizeof((a)[0])) @@ -390,7 +642,6 @@ static int bluetooth_recvmsg_fd(struct bluetooth_data *data) }; ret = recvmsg(data->sock, &msgh, 0); - if (ret < 0) { err = errno; SNDERR("Unable to receive fd: %s (%d)", strerror(err), err); @@ -415,102 +666,143 @@ static int bluetooth_recvmsg_fd(struct bluetooth_data *data) return -EINVAL; } -static int bluetooth_cfg(struct bluetooth_data *data) +static int bluetooth_a2dp_init(struct bluetooth_data *data, + struct ipc_codec_sbc *sbc) { - int ret, len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg); - struct ipc_packet *pkt; + struct bluetooth_a2dp *a2dp = &data->a2dp; + struct ipc_data_cfg *cfg = &data->cfg; - DBG("Sending PKT_TYPE_CFG_REQ..."); + if (cfg == NULL) { + SNDERR("Error getting codec parameters"); + return -1; + } - if ((pkt = malloc(len)) == 0) - return -ENOMEM; + if (cfg->codec != CFG_CODEC_SBC) + return -1; + + /* FIXME: init using flags? */ + sbc_init(&a2dp->sbc, 0); + a2dp->sbc.rate = cfg->rate; + a2dp->sbc.channels = cfg->channels; + if (cfg->channel_mode == CFG_CHANNEL_MODE_MONO || + cfg->channel_mode == CFG_CHANNEL_MODE_JOINT_STEREO) + a2dp->sbc.joint = 1; + a2dp->sbc.allocation = sbc->allocation; + a2dp->sbc.subbands = sbc->subbands; + a2dp->sbc.blocks = sbc->blocks; + a2dp->sbc.bitpool = sbc->bitpool; - memset(pkt, 0, len); + return 0; +} + +static int bluetooth_cfg(struct bluetooth_data *data, snd_config_t *conf) +{ + int ret, total; + char buf[IPC_MTU]; + struct ipc_packet *pkt = (void *) buf; + struct ipc_data_cfg *cfg = (void *) pkt->data; + struct ipc_codec_sbc *sbc = (void *) cfg->data; + + DBG("Sending PKT_TYPE_CFG_REQ..."); + + memset(buf, 0, sizeof(buf)); pkt->type = PKT_TYPE_CFG_REQ; pkt->role = PKT_ROLE_NONE; pkt->error = PKT_ERROR_NONE; - if ((ret = send(data->sock, pkt, len, 0)) < 0) { - ret = -errno; - goto done; - } else if (ret == 0) { - ret = -EIO; - goto done; - } + ret = send(data->sock, pkt, sizeof(struct ipc_packet), 0); + if (ret < 0) + return -errno; + else if (ret == 0) + return -EIO; - DBG("OK - %d bytes sent", ret); + DBG("OK - %d bytes sent. Waiting for response...", ret); - DBG("Waiting for response..."); + memset(buf, 0, sizeof(buf)); - memset(pkt, 0, len); - if ((ret = recv(data->sock, pkt, len, 0)) < 0) { - ret = -errno; - goto done; - } else if (ret == 0) { - ret = -EIO; - goto done; - } + ret = recv(data->sock, buf, sizeof(*pkt) + sizeof(*cfg), 0); + if (ret < 0) + return -errno; + else if (ret == 0) + return -EIO; - DBG("OK - %d bytes received", ret); + total = ret; if (pkt->type != PKT_TYPE_CFG_RSP) { SNDERR("Unexpected packet type received: type = %d", pkt->type); - ret = -EINVAL; - goto done; + return -EINVAL; } if (pkt->error != PKT_ERROR_NONE) { SNDERR("Error while configuring device: error = %d", pkt->error); - ret = pkt->error; - goto done; + return pkt->error; } - if (pkt->length != sizeof(struct ipc_data_cfg)) { + if (cfg->codec != CFG_CODEC_SBC) + goto done; + + ret = recv(data->sock, sbc, sizeof(*sbc), 0); + if (ret < 0) + return -errno; + else if (ret == 0) + return -EIO; + + total += ret; + +done: + DBG("OK - %d bytes received", total); + + if (pkt->length != (total - sizeof(struct ipc_packet))) { SNDERR("Error while configuring device: packet size doesn't " "match"); - ret = -EINVAL; - goto done; + return -EINVAL; } - memcpy(&data->cfg, pkt->data, sizeof(struct ipc_data_cfg)); + memcpy(&data->cfg, cfg, sizeof(*cfg)); DBG("Device configuration:"); - DBG("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u, sample_size=%u," - "rate=%u", data->cfg.fd, data->cfg.fd_opt, - data->cfg.channels, data->cfg.pkt_len, - data->cfg.sample_size, data->cfg.rate); + DBG("\n\tfd=%d\n\tfd_opt=%u\n\tchannels=%u\n\tpkt_len=%u\n" + "\tsample_size=%u\n\trate=%u", data->cfg.fd, + data->cfg.fd_opt, data->cfg.channels, data->cfg.pkt_len, + data->cfg.sample_size, data->cfg.rate); + + if (data->cfg.codec == CFG_CODEC_SBC) { + struct bluetooth_a2dp *a2dp = &data->a2dp; + ret = bluetooth_a2dp_init(data, sbc); + if (ret < 0) + return ret; + printf("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\t" + "bitpool=%u\n", a2dp->sbc.allocation, + a2dp->sbc.subbands, a2dp->sbc.blocks, + a2dp->sbc.bitpool); + } if (data->cfg.fd == -1) { SNDERR("Error while configuring device: could not acquire " "audio socket"); - ret = -EINVAL; - goto done; + return -EINVAL; } - if ((ret = bluetooth_recvmsg_fd(data)) < 0) - goto done; - - if ((data->buffer = malloc(data->cfg.pkt_len)) == 0) - return -ENOMEM; + ret = bluetooth_recvmsg_fd(data); + if (ret < 0) + return ret; /* It is possible there is some outstanding data in the pipe - we have to empty it */ - while(recv(data->cfg.fd, data->buffer, data->cfg.pkt_len, - MSG_DONTWAIT) > 0); + while (recv(data->cfg.fd, data->buffer, data->cfg.pkt_len, + MSG_DONTWAIT) > 0); memset(data->buffer, 0, data->cfg.pkt_len); -done: - free(pkt); - return ret; + return 0; } -static int bluetooth_init(struct bluetooth_data *data) +static int bluetooth_init(struct bluetooth_data *data, snd_config_t *conf) { - int sk, err, id; + int sk, err; struct sockaddr_un addr = { AF_UNIX, IPC_SOCKET_NAME }; @@ -522,28 +814,24 @@ static int bluetooth_init(struct bluetooth_data *data) data->sock = -1; - id = abs(getpid() * rand()); - - if ((sk = socket(PF_LOCAL, SOCK_STREAM, 0)) < 0) { - err = -errno; - SNDERR("Can't open socket"); - return -errno; + sk = socket(PF_LOCAL, SOCK_STREAM, 0); + if (sk < 0) { + err = errno; + SNDERR("Cannot open socket: %s (%d)", strerror(err), err); + return -err; } DBG("Connecting to address: %s", addr.sun_path + 1); if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { - err = -errno; - SNDERR("Can't connect socket"); + err = errno; + SNDERR("Connection fail", strerror(err), err); close(sk); - return err; + return -err; } data->sock = sk; - if ((err = bluetooth_cfg(data)) < 0) - return err; - - return 0; + return bluetooth_cfg(data, conf); } SND_PCM_PLUGIN_DEFINE_FUNC(bluetooth) @@ -555,27 +843,32 @@ SND_PCM_PLUGIN_DEFINE_FUNC(bluetooth) stream == SND_PCM_STREAM_PLAYBACK ? "Playback" : "Capture"); data = malloc(sizeof(struct bluetooth_data)); - memset(data, 0, sizeof(struct bluetooth_data)); if (!data) { err = -ENOMEM; goto error; } - err = bluetooth_init(data); + err = bluetooth_init(data, conf); if (err < 0) goto error; data->io.version = SND_PCM_IOPLUG_VERSION; data->io.name = "Bluetooth Audio Device"; data->io.mmap_rw = 0; /* No direct mmap communication */ - - data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ? - &bluetooth_playback_callback : &bluetooth_capture_callback; data->io.poll_fd = data->cfg.fd; data->io.poll_events = stream == SND_PCM_STREAM_PLAYBACK ? POLLOUT : POLLIN; data->io.private_data = data; + if (data->cfg.codec == CFG_CODEC_SBC) + data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ? + &bluetooth_a2dp_playback : + &bluetooth_a2dp_capture; + else + data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ? + &bluetooth_hsp_playback : + &bluetooth_hsp_capture; + err = snd_pcm_ioplug_create(&data->io, name, stream, mode); if (err < 0) goto error; diff --git a/audio/sink.c b/audio/sink.c index 436dfbce..f7e32647 100644 --- a/audio/sink.c +++ b/audio/sink.c @@ -25,4 +25,433 @@ #include <config.h> #endif +#include <errno.h> + +#include <glib.h> +#include <dbus/dbus.h> + +#include "dbus.h" +#include "dbus-helper.h" +#include "logging.h" + #include "avdtp.h" +#include "device.h" +#include "a2dp.h" +#include "error.h" +#include "unix.h" + +struct pending_connect { + DBusMessage *msg; + struct ipc_packet *pkt; + int pkt_len; + int sock; +}; + +struct sink { + struct avdtp *session; + struct avdtp_stream *stream; + uint8_t state; + struct pending_connect *c; + DBusConnection *conn; +}; + +static void pending_connect_free(struct pending_connect *c) +{ + if (c->pkt) + g_free(c->pkt); + if (c->msg) + dbus_message_unref(c->msg); + g_free(c); +} + +void stream_state_changed(struct avdtp_stream *stream, avdtp_state_t old_state, + avdtp_state_t new_state, + struct avdtp_error *err, void *user_data) +{ + struct device *dev = user_data; + struct sink *sink = dev->sink; + struct pending_connect *c = NULL; + DBusMessage *reply; + int cmd_err; + + if (err) + goto failed; + + switch (new_state) { + case AVDTP_STATE_IDLE: + dbus_connection_emit_signal(dev->conn, dev->path, + AUDIO_SINK_INTERFACE, + "Disconnected", + DBUS_TYPE_INVALID); + if (sink->session) { + avdtp_unref(sink->session); + sink->session = NULL; + } + c = sink->c; + break; + case AVDTP_STATE_CONFIGURED: + cmd_err = avdtp_open(sink->session, stream); + if (cmd_err < 0) { + error("Error on avdtp_open %s (%d)", strerror(-cmd_err), + cmd_err); + goto failed; + } + break; + case AVDTP_STATE_OPEN: + if (old_state == AVDTP_STATE_CONFIGURED) + dbus_connection_emit_signal(dev->conn, dev->path, + AUDIO_SINK_INTERFACE, + "Connected", + DBUS_TYPE_INVALID); + if (sink->c && sink->c->pkt) { + cmd_err = avdtp_start(sink->session, stream); + if (cmd_err < 0) { + error("Error on avdtp_start %s (%d)", + strerror(-cmd_err), cmd_err); + goto failed; + } + } + else + c = sink->c; + break; + case AVDTP_STATE_STREAMING: + c = sink->c; + break; + case AVDTP_STATE_CLOSING: + break; + case AVDTP_STATE_ABORTING: + break; + } + + sink->state = new_state; + + if (c) { + if (c->msg) { + reply = dbus_message_new_method_return(c->msg); + send_message_and_unref(dev->conn, reply); + } + if (c->pkt) { + struct ipc_data_cfg *rsp; + int ret; + + ret = sink_get_config(dev, c->sock, c->pkt, + c->pkt_len, &rsp); + if (ret == 0) { + unix_send_cfg(c->sock, rsp); + g_free(rsp); + } + else + unix_send_cfg(c->sock, NULL); + } + + pending_connect_free(c); + sink->c = NULL; + } + + return; + +failed: + if (sink->c) { + if (sink->c->msg && err) + err_failed(dev->conn, sink->c->msg, + avdtp_strerror(err)); + + pending_connect_free(sink->c); + sink->c = NULL; + } + + if (new_state == AVDTP_STATE_IDLE) { + avdtp_unref(sink->session); + sink->session = NULL; + } +} + + + +static void discovery_complete(struct avdtp *session, GSList *seps, int err, + void *user_data) +{ + struct device *dev = user_data; + struct sink *sink = dev->sink; + struct avdtp_local_sep *lsep; + struct avdtp_remote_sep *rsep; + GSList *caps = NULL; + const char *err_str = NULL; + + if (err < 0) { + error("Discovery failed"); + err_str = strerror(-err); + goto failed; + } + + debug("Discovery complete"); + + if (avdtp_get_seps(session, AVDTP_SEP_TYPE_SINK, AVDTP_MEDIA_TYPE_AUDIO, + A2DP_CODEC_SBC, &lsep, &rsep) < 0) { + err_str = "No matching ACP and INT SEPs found"; + goto failed; + } + + if (!a2dp_select_capabilities(rsep, &caps)) { + err_str = "Unable to select remote SEP capabilities"; + goto failed; + } + + err = avdtp_set_configuration(session, rsep, lsep, caps, + &sink->stream); + if (err < 0) { + error("avdtp_set_configuration: %s", strerror(-err)); + err_str = "Unable to set configuration"; + goto failed; + } + + avdtp_stream_set_cb(session, sink->stream, stream_state_changed, dev); + + return; + +failed: + error("%s", err_str); + if (sink->c) { + if (sink->c->msg) + err_failed(dev->conn, sink->c->msg, err_str); + pending_connect_free(sink->c); + sink->c = NULL; + } + if (sink->session) { + avdtp_unref(sink->session); + sink->session = NULL; + } +} + +static DBusHandlerResult sink_connect(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + struct device *dev = data; + struct sink *sink = dev->sink; + struct pending_connect *c; + int err; + + if (!sink->session) + sink->session = avdtp_get(&dev->src, &dev->dst); + + if (sink->c) + return err_connect_failed(conn, msg, "Connect in progress"); + + if (sink->state >= AVDTP_STATE_OPEN) + return err_already_connected(conn, msg); + + c = g_new0(struct pending_connect, 1); + c->msg = dbus_message_ref(msg); + sink->c = c; + + err = avdtp_discover(sink->session, discovery_complete, data); + if (err < 0) { + dbus_message_unref(c->msg); + pending_connect_free(c); + sink->c = NULL; + return err_connect_failed(conn, msg, strerror(err)); + } + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusHandlerResult sink_disconnect(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + struct device *device = data; + struct sink *sink = device->sink; + struct pending_connect *c; + int err; + + if (!sink->session) + return err_not_connected(conn, msg); + + if (sink->c) + return err_failed(conn, msg, strerror(EBUSY)); + + if (sink->state < AVDTP_STATE_OPEN) { + avdtp_unref(sink->session); + sink->session = NULL; + } else { + err = avdtp_close(sink->session, sink->stream); + if (err < 0) + return err_failed(conn, msg, strerror(-err)); + } + + c = g_new0(struct pending_connect, 1); + c->msg = dbus_message_ref(msg); + sink->c = c; + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusHandlerResult sink_is_connected(DBusConnection *conn, + DBusMessage *msg, + void *data) +{ + struct device *device = data; + struct sink *sink = device->sink; + DBusMessage *reply; + dbus_bool_t connected; + + reply = dbus_message_new_method_return(msg); + if (!reply) + return DBUS_HANDLER_RESULT_NEED_MEMORY; + + connected = (sink->state != AVDTP_STATE_IDLE); + + dbus_message_append_args(reply, DBUS_TYPE_BOOLEAN, &connected, + DBUS_TYPE_INVALID); + + send_message_and_unref(conn, reply); + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusMethodVTable sink_methods[] = { + { "Connect", sink_connect, "", "" }, + { "Disconnect", sink_disconnect, "", "" }, + { "IsConnected", sink_is_connected, "", "b" }, + { NULL, NULL, NULL, NULL } +}; + +static DBusSignalVTable sink_signals[] = { + { "Connected", "" }, + { "Disconnected", "" }, + { NULL, NULL } +}; + +struct sink *sink_init(void *device) +{ + struct device *dev = device; + + if (!dbus_connection_register_interface(dev->conn, dev->path, + AUDIO_SINK_INTERFACE, + sink_methods, + sink_signals, NULL)) + return NULL; + + return g_new0(struct sink, 1); +} + +void sink_free(void *device) +{ + struct device *dev = device; + struct sink *sink = dev->sink; + + if (sink->session) + avdtp_unref(sink->session); + + if (sink->c) + pending_connect_free(sink->c); + + g_free(sink); + dev->sink = NULL; +} + +int sink_get_config(void *device, int sock, struct ipc_packet *req, + int pkt_len, struct ipc_data_cfg **rsp) +{ + struct device *dev = device; + struct sink *sink = dev->sink; + int err = EINVAL; + struct pending_connect *c = NULL; + + if (sink->state == AVDTP_STATE_STREAMING) + goto proceed; + + if (!sink->session) + sink->session = avdtp_get(&dev->src, &dev->dst); + + c = g_new0(struct pending_connect, 1); + c->sock = sock; + c->pkt = g_malloc(pkt_len); + memcpy(c->pkt, req, pkt_len); + sink->c = c; + + if (sink->state == AVDTP_STATE_IDLE) + err = avdtp_discover(sink->session, discovery_complete, device); + else if (sink->state < AVDTP_STATE_STREAMING) + err = avdtp_start(sink->session, sink->stream); + else + goto error; + + if (err < 0) + goto error; + + return 1; + +proceed: + if (!a2dp_get_config(sink->stream, rsp)) + goto error; + + return 0; + +error: + if (c) + pending_connect_free(c); + return -err; +} + +gboolean sink_is_active(void *device) +{ + struct device *dev = device; + struct sink *sink = dev->sink; + + if (sink->session) + return TRUE; + + return FALSE; +} + +void sink_set_state(void *device, avdtp_state_t state) +{ + struct device *dev = device; + struct sink *sink = dev->sink; + int err = 0; + + if (sink->state == state) + return; + + if (!sink->session || !sink->stream) + goto failed; + + switch (sink->state) { + case AVDTP_STATE_OPEN: + if (state == AVDTP_STATE_STREAMING) { + err = avdtp_start(sink->session, sink->stream); + if (err == 0) + return; + } + else if (state == AVDTP_STATE_IDLE) { + err = avdtp_close(sink->session, sink->stream); + if (err == 0) + return; + } + break; + case AVDTP_STATE_STREAMING: + if (state == AVDTP_STATE_OPEN) { + err = avdtp_suspend(sink->session, sink->stream); + if (err == 0) + return; + } + else if (state == AVDTP_STATE_IDLE) { + err = avdtp_close(sink->session, sink->stream); + if (err == 0) + return; + } + break; + default: + goto failed; + } +failed: + error("%s: Error changing states", dev->path); +} + +avdtp_state_t sink_get_state(void *device) +{ + struct device *dev = device; + struct sink *sink = dev->sink; + + return sink->state; +} diff --git a/audio/sink.h b/audio/sink.h index 4cc3a0e4..9d65e278 100644 --- a/audio/sink.h +++ b/audio/sink.h @@ -21,7 +21,18 @@ * */ +#include "ipc.h" +#include "avdtp.h" + #define AUDIO_SINK_INTERFACE "org.bluez.audio.Sink" struct sink; +struct sink *sink_init(void *device); +void sink_new_stream(void *device, void *lsep); +void sink_free(void *device); +int sink_get_config(void *device, int sock, struct ipc_packet *req, + int pkt_len, struct ipc_data_cfg **rsp); +gboolean sink_is_active(void *device); +void sink_set_state(void *device, avdtp_state_t state); +avdtp_state_t sink_get_state(void *device); diff --git a/audio/unix.c b/audio/unix.c index fbda7ed9..0880f4ff 100644 --- a/audio/unix.c +++ b/audio/unix.c @@ -37,11 +37,28 @@ #include "logging.h" #include "dbus.h" - #include "manager.h" +#include "ipc.h" +#include "unix.h" + +struct unix_client { + struct device *dev; + int sock; +}; + +static GSList *clients = NULL; static int unix_sock = -1; +static int unix_send_state(int sock, struct ipc_packet *pkt); + +static void client_free(struct unix_client *client) +{ + if (client->sock >= 0) + close(client->sock); + g_free(client); +} + /* Pass file descriptor through local domain sockets (AF_LOCAL, formerly AF_UNIX) and the sendmsg() system call with the cmsg_type field of a "struct cmsghdr" set to SCM_RIGHTS and the data being an integer value equal to the handle of the @@ -75,52 +92,120 @@ static int unix_sendmsg_fd(int sock, int fd, struct ipc_packet *pkt) return sendmsg(sock, &msgh, MSG_NOSIGNAL); } -static void cfg_event(int clisk, struct ipc_packet *pkt) +static void cfg_event(struct unix_client *client, struct ipc_packet *pkt, + int len) { - struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data; - struct device *device; + struct ipc_data_cfg *rsp; + struct device *dev; + int ret; - memset(cfg, 0, sizeof(struct ipc_data_cfg)); + dev = manager_get_connected_device(); + if (dev) + goto proceed; - if ((device = manager_default_device())) { - if (device->headset) - headset_get_config(device, clisk, pkt); - } - else - cfg->fd = -1; + dev = manager_default_device(); + if (!dev) + goto failed; - if (cfg->fd != 0) - unix_send_cfg(clisk, pkt); +proceed: + ret = device_get_config(dev, client->sock, pkt, len, &rsp); + if (ret < 0) + goto failed; + + client->dev = dev; + + /* Connecting in progress */ + if (ret == 1) + return; + + unix_send_cfg(client->sock, rsp); + g_free(rsp); + + return; + +failed: + unix_send_cfg(client->sock, NULL); } -static void ctl_event(int clisk, struct ipc_packet *pkt) +static void ctl_event(struct unix_client *client, struct ipc_packet *pkt, + int len) { } -static void state_event(int clisk, struct ipc_packet *pkt) +static void state_event(struct unix_client *client, struct ipc_packet *pkt, + int len) { struct ipc_data_state *state = (struct ipc_data_state *) pkt->data; - struct device *device; + struct device *dev = client->dev; - if (!(device = manager_default_device())) - return; + if (len > sizeof(struct ipc_packet)) + device_set_state(dev, state->state); + else + state->state = device_get_state(dev); - if (device->headset) - headset_set_state(device, state->state); + unix_send_state(client->sock, pkt); +} + +static gboolean client_cb(GIOChannel *chan, GIOCondition cond, gpointer data) +{ + char buf[IPC_MTU]; + struct ipc_packet *pkt = (void *) buf; + struct unix_client *client = data; + int len, len_check; + + if (cond & G_IO_NVAL) + return FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR)) { + debug("Unix client disconnected"); + device_set_state(client->dev, STATE_CONNECTED); + goto failed; + } + + memset(buf, 0, sizeof(buf)); + + len = recv(client->sock, buf, sizeof(buf), 0); + if (len < 0) { + error("recv: %s (%d)", strerror(errno), errno); + goto failed; + } + + len_check = pkt->length + sizeof(struct ipc_packet); + if (len != len_check) { + error("Packet lenght doesn't match"); + goto failed; + } + + switch (pkt->type) { + case PKT_TYPE_CFG_REQ: + info("Package PKT_TYPE_CFG_REQ:%u", pkt->role); + cfg_event(client, pkt, len); + break; + case PKT_TYPE_STATE_REQ: + info("Package PKT_TYPE_STATE_REQ"); + state_event(client, pkt, len); + break; + case PKT_TYPE_CTL_REQ: + info("Package PKT_TYPE_CTL_REQ"); + ctl_event(client, pkt, len); + break; + } - unix_send_status(clisk, pkt); + return TRUE; - g_free(pkt); +failed: + clients = g_slist_remove(clients, client); + client_free(client); + return FALSE; } -static gboolean unix_event(GIOChannel *chan, GIOCondition cond, gpointer data) +static gboolean server_cb(GIOChannel *chan, GIOCondition cond, gpointer data) { struct sockaddr_un addr; socklen_t addrlen; - struct ipc_packet *pkt; - int sk, clisk, len; - - debug("chan %p cond %td data %p", chan, cond, data); + int sk, cli_sk; + struct unix_client *client; + GIOChannel *io; if (cond & G_IO_NVAL) return FALSE; @@ -135,32 +220,22 @@ static gboolean unix_event(GIOChannel *chan, GIOCondition cond, gpointer data) memset(&addr, 0, sizeof(addr)); addrlen = sizeof(addr); - clisk = accept(sk, (struct sockaddr *) &addr, &addrlen); - if (clisk < 0) { + cli_sk = accept(sk, (struct sockaddr *) &addr, &addrlen); + if (cli_sk < 0) { error("accept: %s (%d)", strerror(errno), errno); return TRUE; } - len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg); - pkt = g_malloc0(len); - len = recv(clisk, pkt, len, 0); + debug("Accepted new client connection on unix socket"); - debug("path %s len %d", addr.sun_path + 1, len); + client = g_new(struct unix_client, 1); + client->sock = cli_sk; + clients = g_slist_append(clients, client); - switch (pkt->type) { - case PKT_TYPE_CFG_REQ: - info("Package PKT_TYPE_CFG_REQ:%u", pkt->role); - cfg_event(clisk, pkt); - break; - case PKT_TYPE_STATE_REQ: - info("Package PKT_TYPE_STATE_REQ"); - state_event(clisk, pkt); - break; - case PKT_TYPE_CTL_REQ: - info("Package PKT_TYPE_CTL_REQ"); - ctl_event(clisk, pkt); - break; - } + io = g_io_channel_unix_new(cli_sk); + g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + client_cb, client); + g_io_channel_unref(io); return TRUE; } @@ -194,10 +269,8 @@ int unix_init(void) listen(sk, 1); io = g_io_channel_unix_new(sk); - g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, - unix_event, NULL); - + server_cb, NULL); g_io_channel_unref(io); info("Unix socket created: %d", sk); @@ -207,59 +280,77 @@ int unix_init(void) void unix_exit(void) { + g_slist_foreach(clients, (GFunc) client_free, NULL); + g_slist_free(clients); close(unix_sock); unix_sock = -1; } -int unix_send_cfg(int sock, struct ipc_packet *pkt) +int unix_send_cfg(int sock, struct ipc_data_cfg *cfg) { - struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data; - int len; + char buf[IPC_MTU]; + struct ipc_packet *pkt = (void *) buf; + int len, codec_len; - info("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u, sample_size=%u," - "rate=%u", cfg->fd, cfg->fd_opt, cfg->channels, - cfg->pkt_len, cfg->sample_size, cfg->rate); + memset(buf, 0, sizeof(buf)); pkt->type = PKT_TYPE_CFG_RSP; - pkt->length = sizeof(struct ipc_data_cfg); + + if (!cfg) { + pkt->error = EINVAL; + len = send(sock, pkt, sizeof(struct ipc_packet), 0); + if (len < 0) + error("send: %s (%d)", strerror(errno), errno); + return len; + } + + debug("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u," + "sample_size=%u, rate=%u", cfg->fd, cfg->fd_opt, + cfg->channels, cfg->pkt_len, cfg->sample_size, cfg->rate); + + if (cfg->codec == CFG_CODEC_SBC) + codec_len = sizeof(struct ipc_codec_sbc); + else + codec_len = 0; + pkt->error = PKT_ERROR_NONE; + pkt->length = sizeof(struct ipc_data_cfg) + codec_len; + memcpy(pkt->data, cfg, pkt->length); - len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg); + len = sizeof(struct ipc_packet) + pkt->length; len = send(sock, pkt, len, 0); if (len < 0) - info("Error %s(%d)", strerror(errno), errno); + error("Error %s(%d)", strerror(errno), errno); - info("%d bytes sent", len); + debug("%d bytes sent", len); if (cfg->fd != -1) { len = unix_sendmsg_fd(sock, cfg->fd, pkt); if (len < 0) - info("Error %s(%d)", strerror(errno), errno); - info("%d bytes sent", len); + error("Error %s(%d)", strerror(errno), errno); + debug("%d bytes sent", len); } - g_free(pkt); return 0; } -int unix_send_status(int sock, struct ipc_packet *pkt) +static int unix_send_state(int sock, struct ipc_packet *pkt) { struct ipc_data_state *state = (struct ipc_data_state *) pkt->data; int len; info("status=%u", state->state); - pkt->type = PKT_TYPE_CFG_RSP; + pkt->type = PKT_TYPE_STATE_RSP; pkt->length = sizeof(struct ipc_data_state); pkt->error = PKT_ERROR_NONE; len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_state); len = send(sock, pkt, len, 0); if (len < 0) - info("Error %s(%d)", strerror(errno), errno); + error("Error %s(%d)", strerror(errno), errno); - info("%d bytes sent", len); + debug("%d bytes sent", len); - g_free(pkt); return 0; } diff --git a/audio/unix.h b/audio/unix.h index 32cf4af9..c771b965 100644 --- a/audio/unix.h +++ b/audio/unix.h @@ -24,7 +24,6 @@ #include "ipc.h" int unix_init(void); - void unix_exit(void); -int unix_send_cfg(int sock, struct ipc_packet *pkt); -int unix_send_status(int sock, struct ipc_packet *pkt); + +int unix_send_cfg(int sock, struct ipc_data_cfg *cfg); |