From ca7967882344c7777b598733c686c93a6d084104 Mon Sep 17 00:00:00 2001 From: Daniel Hammer Date: Wed, 10 Jun 2026 16:00:42 +0200 Subject: [PATCH] feat: A2DP bluetooth audio working --- a2dp.c | 3816 ----------------- avdtp.h | 314 -- kernel/src/Api/Audio.hpp | 17 +- kernel/src/Api/BuildNo.hpp | 2 +- kernel/src/Drivers/USB/Bluetooth/A2dp.cpp | 793 +++- kernel/src/Drivers/USB/Bluetooth/A2dp.hpp | 22 +- kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp | 230 + kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp | 19 + .../src/Drivers/USB/Bluetooth/Bluetooth.cpp | 40 +- .../src/Drivers/USB/Bluetooth/Bluetooth.hpp | 8 + kernel/src/Drivers/USB/Bluetooth/Hci.cpp | 97 +- kernel/src/Drivers/USB/Bluetooth/Hci.hpp | 13 + kernel/src/Drivers/USB/Bluetooth/L2cap.cpp | 43 +- kernel/src/Drivers/USB/Bluetooth/L2cap.hpp | 3 + kernel/src/Drivers/USB/Bluetooth/Sbc.cpp | 195 +- kernel/src/Drivers/USB/Bluetooth/Sbc.hpp | 4 +- kernel/src/Drivers/USB/Xhci.cpp | 26 +- kernel/src/Timekeeping/ApicTimer.cpp | 10 + 18 files changed, 1312 insertions(+), 4340 deletions(-) delete mode 100644 a2dp.c delete mode 100644 avdtp.h create mode 100644 kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp diff --git a/a2dp.c b/a2dp.c deleted file mode 100644 index c7e0fc7..0000000 --- a/a2dp.c +++ /dev/null @@ -1,3816 +0,0 @@ -// SPDX-License-Identifier: GPL-2.0-or-later -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2006-2010 Nokia Corporation - * Copyright (C) 2004-2010 Marcel Holtmann - * Copyright (C) 2011 BMW Car IT GmbH. All rights reserved. - * - * - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#define _GNU_SOURCE - -#include -#include -#include -#include - -#include -#include - -#include "bluetooth/bluetooth.h" -#include "bluetooth/sdp.h" -#include "bluetooth/sdp_lib.h" -#include "bluetooth/uuid.h" - -#include "gdbus/gdbus.h" - -#include "src/btd.h" -#include "src/plugin.h" -#include "src/adapter.h" -#include "src/device.h" -#include "src/dbus-common.h" -#include "src/error.h" -#include "src/profile.h" -#include "src/service.h" -#include "src/log.h" -#include "src/sdpd.h" -#include "src/textfile.h" -#include "src/shared/queue.h" -#include "src/shared/timeout.h" -#include "src/shared/util.h" - -#include "btio/btio.h" - -#include "avdtp.h" -#include "sink.h" -#include "source.h" -#include "a2dp.h" -#include "a2dp-codecs.h" -#include "media.h" - -/* The duration that streams without users are allowed to stay in - * STREAMING state. */ -#define SUSPEND_TIMEOUT 5 - -#define AVDTP_PSM 25 - -#define MEDIA_ENDPOINT_INTERFACE "org.bluez.MediaEndpoint1" - -struct a2dp_stream { - struct avdtp *session; - struct avdtp_stream *stream; - unsigned int suspend_timer; - gboolean locked; - gboolean suspending; - gboolean starting; -}; - -struct a2dp_sep { - struct a2dp_server *server; - struct a2dp_endpoint *endpoint; - uint8_t type; - uint8_t codec; - struct avdtp_local_sep *lsep; - struct queue *streams; - gboolean delay_reporting; - void *user_data; - GDestroyNotify destroy; -}; - -struct a2dp_setup_cb { - struct a2dp_setup *setup; - a2dp_discover_cb_t discover_cb; - a2dp_select_cb_t select_cb; - a2dp_config_cb_t config_cb; - a2dp_stream_cb_t resume_cb; - a2dp_stream_cb_t suspend_cb; - guint source_id; - void *user_data; - unsigned int id; -}; - -struct a2dp_setup { - struct a2dp_channel *chan; - struct avdtp *session; - struct queue *eps; - struct a2dp_sep *sep; - struct a2dp_remote_sep *rsep; - struct avdtp_stream *stream; - struct avdtp_error *err; - avdtp_set_configuration_cb setconf_cb; - GSList *seps; - GSList *caps; - gboolean reconfigure; - gboolean start; - GSList *cb; - GIOChannel *io; - guint id; - int ref; -}; - -struct a2dp_server { - struct btd_adapter *adapter; - GSList *sinks; - GSList *sources; - uint32_t source_record_id; - uint32_t sink_record_id; - gboolean sink_enabled; - gboolean source_enabled; - uint64_t seid_pool; - GIOChannel *io; - struct queue *seps; - struct queue *channels; -}; - -struct a2dp_remote_sep { - struct a2dp_channel *chan; - char *path; - struct avdtp_remote_sep *sep; - bool from_cache; -}; - -struct a2dp_last_used { - struct a2dp_sep *lsep; - struct a2dp_remote_sep *rsep; -}; - -struct a2dp_channel { - struct a2dp_server *server; - struct btd_device *device; - GIOChannel *io; - guint io_id; - unsigned int state_id; - unsigned int auth_id; - struct avdtp *session; - struct queue *seps; - struct a2dp_last_used *last_used; -}; - -static GSList *servers = NULL; -static GSList *setups = NULL; -static unsigned int cb_id = 0; - -struct a2dp_error { - const char *error_name; - uint8_t error_code; -}; - -#define A2DP_ERROR_PREFIX ERROR_INTERFACE ".A2DP." - -static struct a2dp_error config_errors[] = { - {"InvalidCodecType", A2DP_INVALID_CODEC_TYPE}, - {"NotSupportedCodecType", A2DP_NOT_SUPPORTED_CODEC_TYPE}, - {"InvalidSamplingFrequency", A2DP_INVALID_SAMPLING_FREQUENCY}, - {"NotSupportedSamplingFrequency", - A2DP_NOT_SUPPORTED_SAMPLING_FREQUENCY}, - {"InvalidChannelMode", A2DP_INVALID_CHANNEL_MODE}, - {"NotSupportedChannelMode", A2DP_NOT_SUPPORTED_CHANNEL_MODE}, - {"InvalidSubbands", A2DP_INVALID_SUBBANDS}, - {"NotSupportedSubbands", A2DP_NOT_SUPPORTED_SUBBANDS}, - {"InvalidAllocationMethod", A2DP_INVALID_ALLOCATION_METHOD}, - {"NotSupportedAllocationMethod", A2DP_NOT_SUPPORTED_ALLOCATION_METHOD}, - {"InvalidMinimumBitpoolValue", - A2DP_INVALID_MINIMUM_BITPOOL_VALUE}, - {"NotSupportedMinimumBitpoolValue", - A2DP_NOT_SUPPORTED_MINIMUM_BITPOOL_VALUE}, - {"InvalidMaximumBitpoolValue", A2DP_INVALID_MAXIMUM_BITPOOL_VALUE}, - {"NotSupportedMaximumBitpoolValue", - A2DP_NOT_SUPPORTED_MAXIMUM_BITPOOL_VALUE}, - {"InvalidLayer", A2DP_INVALID_LAYER}, - {"NotSupportedLayer", A2DP_NOT_SUPPORTED_LAYER}, - {"NotSupportedCRC", A2DP_NOT_SUPPORTED_CRC}, - {"NotSupportedMPF", A2DP_NOT_SUPPORTED_MPF}, - {"NotSupportedVBR", A2DP_NOT_SUPPORTED_VBR}, - {"InvalidBitRate", A2DP_INVALID_BIT_RATE}, - {"NotSupportedBitRate", A2DP_NOT_SUPPORTED_BIT_RATE}, - {"InvalidObjectType", A2DP_INVALID_OBJECT_TYPE}, - {"NotSupportedObjectType", A2DP_NOT_SUPPORTED_OBJECT_TYPE}, - {"InvalidChannels", A2DP_INVALID_CHANNELS}, - {"NotSupportedChannels", A2DP_NOT_SUPPORTED_CHANNELS}, - {"InvalidVersion", A2DP_INVALID_VERSION}, - {"NotSupportedVersion", A2DP_NOT_SUPPORTED_VERSION}, - {"NotSupportedMaximumSUL", A2DP_NOT_SUPPORTED_MAXIMUM_SUL}, - {"InvalidBlockLength", A2DP_INVALID_BLOCK_LENGTH}, - {"InvalidCPType", A2DP_INVALID_CP_TYPE}, - {"InvalidCPFormat", A2DP_INVALID_CP_FORMAT}, - {"InvalidCodecParameter", A2DP_INVALID_CODEC_PARAMETER}, - {"NotSupportedCodecParameter", A2DP_NOT_SUPPORTED_CODEC_PARAMETER}, - {"InvalidDRC", A2DP_INVALID_DRC}, - {"NotSupportedDRC", A2DP_NOT_SUPPORTED_DRC} -}; - -uint8_t a2dp_parse_config_error(const char *error_name) -{ - size_t prefix_length; - size_t i; - - prefix_length = strlen(A2DP_ERROR_PREFIX); - if (strncmp(A2DP_ERROR_PREFIX, error_name, prefix_length)) - return AVDTP_UNSUPPORTED_CONFIGURATION; - - error_name += prefix_length; - for (i = 0; i < ARRAY_SIZE(config_errors); i++) { - if (strcmp(config_errors[i].error_name, error_name) == 0) - return config_errors[i].error_code; - } - - return AVDTP_UNSUPPORTED_CONFIGURATION; -} - -static struct a2dp_setup *setup_ref(struct a2dp_setup *setup) -{ - setup->ref++; - - DBG("%p: ref=%d", setup, setup->ref); - - return setup; -} - -static bool match_by_session(const void *data, const void *user_data) -{ - const struct a2dp_channel *chan = data; - const struct avdtp *session = user_data; - - return chan->session == session; -} - -static struct a2dp_channel *find_channel(struct avdtp *session) -{ - GSList *l; - - for (l = servers; l; l = g_slist_next(l)) { - struct a2dp_server *server = l->data; - struct a2dp_channel *chan; - - chan = queue_find(server->channels, match_by_session, session); - if (chan) - return chan; - } - - return NULL; -} - -static struct a2dp_setup *setup_new(struct avdtp *session) -{ - struct a2dp_setup *setup; - struct a2dp_channel *chan; - - chan = find_channel(session); - if (!chan) - return NULL; - - setup = g_new0(struct a2dp_setup, 1); - setup->session = avdtp_ref(session); - setup->chan = find_channel(session); - setups = g_slist_append(setups, setup); - - return setup; -} - -static void setup_free(struct a2dp_setup *s) -{ - DBG("%p", s); - - if (s->io) { - g_io_channel_shutdown(s->io, TRUE, NULL); - g_io_channel_unref(s->io); - } - - if (s->id) - g_source_remove(s->id); - - queue_destroy(s->eps, NULL); - - setups = g_slist_remove(setups, s); - if (s->session) - avdtp_unref(s->session); - g_slist_free_full(s->cb, g_free); - g_slist_free_full(s->caps, g_free); - g_free(s->err); - g_free(s); -} - -static void setup_unref(struct a2dp_setup *setup) -{ - setup->ref--; - - DBG("%p: ref=%d", setup, setup->ref); - - if (setup->ref > 0) - return; - - setup_free(setup); -} - -static struct a2dp_setup_cb *setup_cb_new(struct a2dp_setup *setup) -{ - struct a2dp_setup_cb *cb; - - cb = g_new0(struct a2dp_setup_cb, 1); - cb->setup = setup; - cb->id = ++cb_id; - - return cb; -} - -static struct a2dp_setup_cb *setup_cb_add(struct a2dp_setup *setup) -{ - struct a2dp_setup_cb *cb; - - cb = setup_cb_new(setup); - - setup->cb = g_slist_append(setup->cb, cb); - return cb; -} - -static void setup_cb_free(struct a2dp_setup_cb *cb) -{ - struct a2dp_setup *setup = cb->setup; - - if (cb->source_id) - g_source_remove(cb->source_id); - - setup->cb = g_slist_remove(setup->cb, cb); - setup_unref(cb->setup); - g_free(cb); -} - -static void setup_error_set(struct a2dp_setup *setup, struct avdtp_error *err) -{ - if (!err) { - g_free(setup->err); - setup->err = NULL; - } else { - if (!setup->err) - setup->err = g_new0(struct avdtp_error, 1); - memcpy(setup->err, err, sizeof(struct avdtp_error)); - } -} - -static void setup_error_init(struct a2dp_setup *setup, uint8_t type, int id) -{ - struct avdtp_error err; - - avdtp_error_init(&err, type, id); - setup_error_set(setup, &err); -} - -static void finalize_setup_errno(struct a2dp_setup *s, int err, - GSourceFunc cb1, ...) -{ - GSourceFunc finalize; - va_list args; - - if (err < 0) - setup_error_init(s, AVDTP_ERRNO, -err); - - va_start(args, cb1); - finalize = cb1; - setup_ref(s); - while (finalize != NULL) { - finalize(s); - finalize = va_arg(args, GSourceFunc); - } - setup_unref(s); - va_end(args); -} - -static int error_to_errno(struct avdtp_error *err) -{ - int perr; - - if (!err) - return 0; - - if (avdtp_error_category(err) != AVDTP_ERRNO) - return -EIO; - - perr = avdtp_error_posix_errno(err); - switch (perr) { - case EHOSTDOWN: - case ECONNABORTED: - case EBADE: - case ECONNREFUSED: - return -perr; - default: - /* An unexpected error has occurred setup may be attempted - * again. - */ - return -EAGAIN; - } -} - -static gboolean finalize_config(gpointer data) -{ - struct a2dp_setup *s = data; - GSList *l; - struct avdtp_stream *stream = s->err ? NULL : s->stream; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (!cb->config_cb) - continue; - - cb->config_cb(s->session, s->sep, stream, - error_to_errno(s->err), cb->user_data); - setup_cb_free(cb); - } - - return FALSE; -} - -static gboolean finalize_resume(gpointer data) -{ - struct a2dp_setup *s = data; - GSList *l; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (!cb->resume_cb) - continue; - - cb->resume_cb(s->session, error_to_errno(s->err), - cb->user_data); - setup_cb_free(cb); - } - - return FALSE; -} - -static gboolean finalize_suspend(gpointer data) -{ - struct a2dp_setup *s = data; - GSList *l; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (!cb->suspend_cb) - continue; - - cb->suspend_cb(s->session, error_to_errno(s->err), - cb->user_data); - setup_cb_free(cb); - } - - return FALSE; -} - -static void finalize_select(struct a2dp_setup *s) -{ - GSList *l; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (!cb->select_cb) - continue; - - cb->select_cb(s->session, s->sep, s->caps, - error_to_errno(s->err), cb->user_data); - setup_cb_free(cb); - } -} - -static void finalize_discover(struct a2dp_setup *s) -{ - GSList *l; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (!cb->discover_cb) - continue; - - cb->discover_cb(s->session, s->seps, error_to_errno(s->err), - cb->user_data); - setup_cb_free(cb); - } -} - -static gboolean finalize_all(gpointer data) -{ - struct a2dp_setup *s = data; - struct avdtp_stream *stream = s->err ? NULL : s->stream; - GSList *l; - - for (l = s->cb; l != NULL; ) { - struct a2dp_setup_cb *cb = l->data; - - l = l->next; - - if (cb->discover_cb) { - cb->discover_cb(s->session, s->seps, - error_to_errno(s->err), cb->user_data); - } else if (cb->select_cb) { - cb->select_cb(s->session, s->sep, s->caps, - error_to_errno(s->err), cb->user_data); - } else if (cb->suspend_cb) { - cb->suspend_cb(s->session, - error_to_errno(s->err), cb->user_data); - } else if (cb->resume_cb) { - cb->resume_cb(s->session, - error_to_errno(s->err), cb->user_data); - } else if (cb->config_cb) { - cb->config_cb(s->session, s->sep, stream, - error_to_errno(s->err), cb->user_data); - } else - warn("setup_cb doesn't have any callback function"); - - setup_cb_free(cb); - } - - return FALSE; -} - -static struct a2dp_setup *find_setup_by_session(struct avdtp *session) -{ - GSList *l; - - for (l = setups; l != NULL; l = l->next) { - struct a2dp_setup *setup = l->data; - - if (setup->session == session) - return setup; - } - - return NULL; -} - -static struct a2dp_setup *a2dp_setup_get(struct avdtp *session) -{ - struct a2dp_setup *setup; - - setup = find_setup_by_session(session); - if (!setup) { - setup = setup_new(session); - if (!setup) - return NULL; - } - - return setup_ref(setup); -} - -static struct a2dp_setup *find_setup_by_stream(struct avdtp_stream *stream) -{ - GSList *l; - - for (l = setups; l != NULL; l = l->next) { - struct a2dp_setup *setup = l->data; - - if (setup->stream == stream) - return setup; - } - - return NULL; -} - -static bool match_stream_session(const void *data, const void *user_data) -{ - const struct a2dp_stream *stream = data; - const struct avdtp *session = user_data; - - return stream->session == session; -} - -static struct a2dp_stream *a2dp_stream_new(struct a2dp_sep *sep, - struct avdtp *session) -{ - struct a2dp_stream *stream; - - if (!sep->streams) - sep->streams = queue_new(); - - stream = new0(struct a2dp_stream, 1); - stream->session = avdtp_ref(session); - queue_push_tail(sep->streams, stream); - - return stream; -} - -static struct a2dp_stream *a2dp_stream_get(struct a2dp_sep *sep, - struct avdtp *session) -{ - struct a2dp_stream *stream; - - DBG("sep %p session %p", sep, session); - - stream = queue_find(sep->streams, match_stream_session, session); - if (stream) - return stream; - - return a2dp_stream_new(sep, session); -} - -static void a2dp_stream_starting(struct a2dp_sep *sep, struct avdtp *session) -{ - struct a2dp_stream *stream; - - stream = a2dp_stream_get(sep, session); - if (!stream) - return; - - stream->starting = TRUE; -} - -static void a2dp_stream_free(void *data) -{ - struct a2dp_stream *stream = data; - - if (stream->suspend_timer) { - timeout_remove(stream->suspend_timer); - stream->suspend_timer = 0; - } - - avdtp_unref(stream->session); - free(stream); -} - -static bool match_stream(const void *data, const void *user_data) -{ - const struct a2dp_stream *astream = data; - const struct avdtp_stream *stream = user_data; - - return astream->stream == stream; -} - -static void a2dp_stream_destroy(struct a2dp_sep *sep, - struct avdtp_stream *stream) -{ - struct a2dp_stream *astream; - - DBG("sep %p stream %p", sep, stream); - - astream = queue_remove_if(sep->streams, match_stream, stream); - if (astream) - a2dp_stream_free(astream); -} - -static 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 a2dp_sep *sep = user_data; - struct a2dp_stream *a2dp_stream; - struct btd_device *dev = NULL; - - if (new_state == AVDTP_STATE_OPEN) { - struct a2dp_setup *setup; - int err; - - setup = find_setup_by_stream(stream); - if (!setup || !setup->start || setup->err) - return; - - setup->start = FALSE; - - err = avdtp_start(setup->session, stream); - if (err < 0 && err != -EINPROGRESS) { - error("avdtp_start: %s (%d)", strerror(-err), -err); - finalize_setup_errno(setup, err, finalize_resume, - NULL); - return; - } - - a2dp_stream_starting(sep, setup->session); - - return; - } - - if (new_state != AVDTP_STATE_IDLE) - return; - - a2dp_stream = queue_find(sep->streams, match_stream, stream); - if (a2dp_stream) { - dev = avdtp_get_device(a2dp_stream->session); - a2dp_stream_destroy(sep, stream); - } - - if (sep->endpoint && sep->endpoint->clear_configuration) - sep->endpoint->clear_configuration(sep, dev, sep->user_data); -} - -static gboolean auto_config(gpointer data) -{ - struct a2dp_setup *setup = data; - struct btd_device *dev = NULL; - struct btd_service *service; - struct a2dp_stream *stream; - - dev = avdtp_get_device(setup->session); - - if (setup->sep->type == AVDTP_SEP_TYPE_SOURCE) - service = btd_device_get_service(dev, A2DP_SINK_UUID); - else - service = btd_device_get_service(dev, A2DP_SOURCE_UUID); - - if (service == NULL) { - error("Unable to find btd service"); - return FALSE; - } - - /* Check if configuration was aborted */ - stream = queue_find(setup->sep->streams, match_stream, setup->stream); - if (!stream) - return FALSE; - - if (setup->err != NULL) - goto done; - - avdtp_stream_add_cb(setup->session, setup->stream, - stream_state_changed, setup->sep); - - if (setup->sep->type == AVDTP_SEP_TYPE_SOURCE) { - sink_new_stream(service, setup->session, setup->stream); - } else { - source_new_stream(service, setup->session, setup->stream); - } - -done: - if (setup->setconf_cb) - setup->setconf_cb(setup->session, setup->stream, setup->err); - - finalize_config(setup); - - setup_error_set(setup, NULL); - - setup_unref(setup); - - return FALSE; -} - -static void endpoint_setconf_cb(struct a2dp_setup *setup, uint8_t error_code) -{ - if (error_code != 0) - setup_error_init(setup, AVDTP_MEDIA_CODEC, error_code); - - auto_config(setup); - setup_unref(setup); -} - -static gboolean endpoint_match_codec_ind(struct avdtp *session, - struct avdtp_media_codec_capability *codec, - void *user_data) -{ - struct a2dp_sep *sep = user_data; - a2dp_vendor_codec_t *remote_codec; - a2dp_vendor_codec_t *local_codec; - uint8_t *capabilities; - size_t length; - - if (codec->media_codec_type != A2DP_CODEC_VENDOR) - return TRUE; - - if (sep->endpoint == NULL) - return FALSE; - - length = sep->endpoint->get_capabilities(sep, &capabilities, - sep->user_data); - if (length < sizeof(a2dp_vendor_codec_t)) - return FALSE; - - local_codec = (a2dp_vendor_codec_t *) capabilities; - remote_codec = (a2dp_vendor_codec_t *) codec->data; - - if (A2DP_GET_VENDOR_ID(*remote_codec) != - A2DP_GET_VENDOR_ID(*local_codec)) - return FALSE; - - if (A2DP_GET_CODEC_ID(*remote_codec) != A2DP_GET_CODEC_ID(*local_codec)) - return FALSE; - - DBG("vendor 0x%08x codec 0x%04x", A2DP_GET_VENDOR_ID(*remote_codec), - A2DP_GET_CODEC_ID(*remote_codec)); - return TRUE; -} - -static void reverse_discover(struct avdtp *session, GSList *seps, int err, - void *user_data) -{ - DBG("err %d", err); -} - -static gboolean endpoint_setconf_ind(struct avdtp *session, - struct avdtp_local_sep *sep, - struct avdtp_stream *stream, - GSList *caps, - avdtp_set_configuration_cb cb, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - struct a2dp_stream *a2dp_stream; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Set_Configuration_Ind", sep); - else - DBG("Source %p: Set_Configuration_Ind", sep); - - a2dp_stream = a2dp_stream_get(a2dp_sep, session); - if (!a2dp_stream) - return FALSE; - - setup = a2dp_setup_get(session); - if (!setup) - return FALSE; - - a2dp_stream->stream = stream; - setup->sep = a2dp_sep; - setup->stream = stream; - setup->setconf_cb = cb; - - for (; caps != NULL; caps = g_slist_next(caps)) { - struct avdtp_service_capability *cap = caps->data; - struct avdtp_media_codec_capability *codec; - gboolean ret; - - if (cap->category == AVDTP_DELAY_REPORTING && - !a2dp_sep->delay_reporting) { - setup_error_init(setup, AVDTP_DELAY_REPORTING, - AVDTP_UNSUPPORTED_CONFIGURATION); - goto done; - } - - if (cap->category != AVDTP_MEDIA_CODEC) - continue; - - codec = (struct avdtp_media_codec_capability *) cap->data; - - if (codec->media_codec_type != a2dp_sep->codec) { - setup_error_init(setup, AVDTP_MEDIA_CODEC, - AVDTP_UNSUPPORTED_CONFIGURATION); - goto done; - } - - ret = a2dp_sep->endpoint->set_configuration(a2dp_sep, - codec->data, - cap->length - sizeof(*codec), - setup_ref(setup), - endpoint_setconf_cb, - a2dp_sep->user_data); - if (ret == 0) { - /* Attempt to reverse discover if there are no remote - * SEPs. - */ - if (queue_isempty(setup->chan->seps)) - a2dp_discover(session, reverse_discover, NULL); - return TRUE; - } - - setup_error_init(setup, AVDTP_MEDIA_CODEC, - AVDTP_UNSUPPORTED_CONFIGURATION); - setup_unref(setup); - break; - } - -done: - g_idle_add(auto_config, setup); - return TRUE; -} - -static gboolean endpoint_getcap_ind(struct avdtp *session, - struct avdtp_local_sep *sep, - gboolean get_all, GSList **caps, - uint8_t *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct avdtp_service_capability *media_transport, *media_codec; - struct avdtp_media_codec_capability *codec_caps; - uint8_t *capabilities; - size_t length; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Get_Capability_Ind", sep); - else - DBG("Source %p: Get_Capability_Ind", sep); - - *caps = NULL; - - media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT, - NULL, 0); - - *caps = g_slist_append(*caps, media_transport); - - length = a2dp_sep->endpoint->get_capabilities(a2dp_sep, &capabilities, - a2dp_sep->user_data); - - codec_caps = g_malloc0(sizeof(*codec_caps) + length); - codec_caps->media_type = AVDTP_MEDIA_TYPE_AUDIO; - codec_caps->media_codec_type = a2dp_sep->codec; - memcpy(codec_caps->data, capabilities, length); - - media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, codec_caps, - sizeof(*codec_caps) + length); - - *caps = g_slist_append(*caps, media_codec); - g_free(codec_caps); - - if (get_all) { - struct avdtp_service_capability *delay_reporting; - delay_reporting = avdtp_service_cap_new(AVDTP_DELAY_REPORTING, - NULL, 0); - *caps = g_slist_append(*caps, delay_reporting); - } - - return TRUE; -} - -static void endpoint_open_cb(struct a2dp_setup *setup, uint8_t error_code) -{ - int err = error_to_errno(setup->err); - - if (error_code != 0) { - setup->stream = NULL; - finalize_setup_errno(setup, -EPERM, finalize_config, NULL); - goto done; - } - - if (err == 0) - err = avdtp_open(setup->session, setup->stream); - - if (err == 0) - goto done; - - error("avdtp_open %s (%d)", strerror(-err), -err); - setup->stream = NULL; - finalize_setup_errno(setup, err, finalize_config, NULL); -done: - setup_unref(setup); -} - -static bool match_remote_sep(const void *data, const void *user_data) -{ - const struct a2dp_remote_sep *sep = data; - const struct avdtp_remote_sep *rsep = user_data; - - return sep->sep == rsep; -} - -static void store_remote_sep(void *data, void *user_data) -{ - struct a2dp_remote_sep *sep = data; - GKeyFile *key_file = user_data; - char seid[4], value[256]; - struct avdtp_service_capability *service = avdtp_get_codec(sep->sep); - struct avdtp_media_codec_capability *codec; - unsigned int i; - ssize_t offset; - - if (!service) - return; - - codec = (void *) service->data; - - sprintf(seid, "%02hhx", avdtp_get_seid(sep->sep)); - - offset = sprintf(value, "%02hhx:%02hhx:%02hhx:", - avdtp_get_type(sep->sep), codec->media_codec_type, - avdtp_get_delay_reporting(sep->sep)); - - for (i = 0; i < service->length - sizeof(*codec); i++) - offset += sprintf(value + offset, "%02hhx", codec->data[i]); - - g_key_file_set_string(key_file, "Endpoints", seid, value); -} - -static void store_remote_seps(struct a2dp_channel *chan) -{ - struct btd_device *device = chan->device; - char filename[PATH_MAX]; - char dst_addr[18]; - GKeyFile *key_file; - GError *gerr = NULL; - char *data; - gsize length = 0; - - ba2str(device_get_address(device), dst_addr); - - create_filename(filename, PATH_MAX, "/%s/cache/%s", - btd_adapter_get_storage_dir(device_get_adapter(device)), - dst_addr); - - key_file = g_key_file_new(); - if (!g_key_file_load_from_file(key_file, filename, 0, &gerr)) { - error("Unable to load key file from %s: (%s)", filename, - gerr->message); - g_clear_error(&gerr); - } - - data = g_key_file_get_string(key_file, "Endpoints", "LastUsed", - NULL); - - /* Remove current endpoints since it might have changed */ - g_key_file_remove_group(key_file, "Endpoints", NULL); - - queue_foreach(chan->seps, store_remote_sep, key_file); - - if (data) { - g_key_file_set_string(key_file, "Endpoints", "LastUsed", - data); - g_free(data); - } - - data = g_key_file_to_data(key_file, &length, NULL); - if (!g_file_set_contents(filename, data, length, &gerr)) { - error("Unable set contents for %s: (%s)", filename, - gerr->message); - g_error_free(gerr); - } - - g_free(data); - g_key_file_free(key_file); -} - -static void invalidate_remote_cache(struct a2dp_setup *setup, - struct avdtp_error *err) -{ - if (err->category == AVDTP_ERRNO) - return; - - /* Attempt to unregister Remote SEP if configuration fails and it was - * loaded from cache. - */ - if (setup->rsep && setup->rsep->from_cache) { - warn("Invalidating Remote SEP from cache"); - avdtp_unregister_remote_sep(setup->session, setup->rsep->sep); - /* Update cache */ - store_remote_seps(setup->chan); - /* Set error to -EAGAIN so the likes of policy plugin can - * reattempt to connect. - */ - setup_error_init(setup, AVDTP_ERRNO, -EAGAIN); - } -} - -static void setconf_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - struct a2dp_stream *a2dp_stream; - struct btd_device *dev; - struct btd_service *service; - int ret; - - dev = avdtp_get_device(session); - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) { - DBG("Sink %p: Set_Configuration_Cfm", sep); - service = btd_device_get_service(dev, A2DP_SOURCE_UUID); - } else { - DBG("Source %p: Set_Configuration_Cfm", sep); - service = btd_device_get_service(dev, A2DP_SINK_UUID); - } - - if (service == NULL) { - error("Unable to find btd service"); - return; - } - - setup = find_setup_by_session(session); - - if (err) { - if (setup) { - setup_ref(setup); - setup_error_set(setup, err); - invalidate_remote_cache(setup, err); - finalize_config(setup); - setup_error_set(setup, NULL); - setup_unref(setup); - } - - return; - } - - avdtp_stream_add_cb(session, stream, stream_state_changed, a2dp_sep); - - a2dp_stream = a2dp_stream_get(a2dp_sep, session); - if (a2dp_stream) - a2dp_stream->stream = stream; - - if (!setup) - return; - - /* Notify D-Bus interface of the new stream */ - if (a2dp_sep->type == AVDTP_SEP_TYPE_SOURCE) { - sink_new_stream(service, session, setup->stream); - } else { - source_new_stream(service, session, setup->stream); - } - - /* Notify Endpoint */ - if (a2dp_sep->endpoint) { - struct avdtp_service_capability *service; - struct avdtp_media_codec_capability *codec; - int err; - - service = avdtp_stream_get_codec(stream); - codec = (struct avdtp_media_codec_capability *) service->data; - - err = a2dp_sep->endpoint->set_configuration(a2dp_sep, - codec->data, service->length - - sizeof(*codec), - setup_ref(setup), - endpoint_open_cb, - a2dp_sep->user_data); - if (err == 0) - return; - - setup->stream = NULL; - finalize_setup_errno(setup, -EPERM, finalize_config, NULL); - setup_unref(setup); - return; - } - - ret = avdtp_open(session, stream); - if (ret < 0) { - error("avdtp_open %s (%d)", strerror(-ret), -ret); - setup->stream = NULL; - finalize_setup_errno(setup, ret, finalize_config, NULL); - } -} - -static gboolean getconf_ind(struct avdtp *session, struct avdtp_local_sep *sep, - uint8_t *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Get_Configuration_Ind", sep); - else - DBG("Source %p: Get_Configuration_Ind", sep); - return TRUE; -} - -static void getconf_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Set_Configuration_Cfm", sep); - else - DBG("Source %p: Set_Configuration_Cfm", sep); -} - -static void store_last_used(struct a2dp_channel *chan, uint8_t lseid, - uint8_t rseid) -{ - GKeyFile *key_file; - GError *gerr = NULL; - char filename[PATH_MAX]; - char dst_addr[18]; - char value[6]; - char *data; - size_t len = 0; - - ba2str(device_get_address(chan->device), dst_addr); - - create_filename(filename, PATH_MAX, "/%s/cache/%s", - btd_adapter_get_storage_dir(device_get_adapter(chan->device)), - dst_addr); - - key_file = g_key_file_new(); - if (!g_key_file_load_from_file(key_file, filename, 0, &gerr)) { - error("Unable to load key file from %s: (%s)", filename, - gerr->message); - g_clear_error(&gerr); - } - - sprintf(value, "%02hhx:%02hhx", lseid, rseid); - - g_key_file_set_string(key_file, "Endpoints", "LastUsed", value); - - data = g_key_file_to_data(key_file, &len, NULL); - if (!g_file_set_contents(filename, data, len, &gerr)) { - error("Unable set contents for %s: (%s)", filename, - gerr->message); - g_error_free(gerr); - } - - g_free(data); - g_key_file_free(key_file); -} - -static void add_last_used(struct a2dp_channel *chan, struct a2dp_sep *lsep, - struct a2dp_remote_sep *rsep) -{ - if (!chan->last_used) - chan->last_used = new0(struct a2dp_last_used, 1); - - chan->last_used->lsep = lsep; - chan->last_used->rsep = rsep; -} - -static void update_last_used(struct a2dp_channel *chan, struct a2dp_sep *lsep, - struct avdtp_stream *stream) -{ - struct avdtp_remote_sep *rsep; - struct a2dp_remote_sep *sep; - - rsep = avdtp_stream_get_remote_sep(stream); - sep = queue_find(chan->seps, match_remote_sep, rsep); - if (!sep) { - error("Unable to find remote SEP"); - return; - } - - /* Check if already stored then skip */ - if (chan->last_used && (chan->last_used->lsep == lsep && - chan->last_used->rsep == sep)) - return; - - add_last_used(chan, lsep, sep); - - store_last_used(chan, avdtp_sep_get_seid(lsep->lsep), - avdtp_get_seid(rsep)); -} - -static gboolean open_ind(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Open_Ind", sep); - else - DBG("Source %p: Open_Ind", sep); - - setup = a2dp_setup_get(session); - if (!setup) - return FALSE; - - setup->stream = stream; - - if (!err && setup->chan) - update_last_used(setup->chan, a2dp_sep, stream); - - if (setup->reconfigure) - setup->reconfigure = FALSE; - - finalize_config(setup); - - return TRUE; -} - -static void open_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Open_Cfm", sep); - else - DBG("Source %p: Open_Cfm", sep); - - setup = find_setup_by_session(session); - if (!setup) - return; - - if (setup->reconfigure) - setup->reconfigure = FALSE; - - if (err) { - setup->stream = NULL; - setup_error_set(setup, err); - if (setup->start) - finalize_resume(setup); - } else if (setup->chan) - update_last_used(setup->chan, a2dp_sep, stream); - - finalize_config(setup); - - return; -} - -static bool suspend_timeout(struct a2dp_stream *stream) -{ - if (avdtp_suspend(stream->session, stream->stream) == 0) - stream->suspending = TRUE; - - stream->suspend_timer = 0; - - return FALSE; -} - -static gboolean start_ind(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_stream *a2dp_stream; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Start_Ind", sep); - else - DBG("Source %p: Start_Ind", sep); - - a2dp_stream = queue_find(a2dp_sep->streams, match_stream, stream); - if (!a2dp_stream) - return FALSE; - - if (!a2dp_stream->locked) - a2dp_stream->suspend_timer = timeout_add_seconds( - SUSPEND_TIMEOUT, - (timeout_func_t) suspend_timeout, - a2dp_stream, NULL); - - if (!a2dp_stream->starting) - return TRUE; - - a2dp_stream->starting = FALSE; - - setup = find_setup_by_session(session); - if (setup) - finalize_resume(setup); - - return TRUE; -} - -static void start_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_stream *a2dp_stream; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Start_Cfm", sep); - else - DBG("Source %p: Start_Cfm", sep); - - a2dp_stream = queue_find(a2dp_sep->streams, match_stream, stream); - if (!a2dp_stream) - return; - - a2dp_stream->starting = FALSE; - - setup = find_setup_by_session(session); - if (!setup) - return; - - if (err) { - setup->stream = NULL; - setup_error_set(setup, err); - } - - finalize_resume(setup); -} - -static gboolean suspend_ind(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_stream *a2dp_stream; - struct a2dp_setup *setup; - gboolean start; - int start_err; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Suspend_Ind", sep); - else - DBG("Source %p: Suspend_Ind", sep); - - a2dp_stream = queue_find(a2dp_sep->streams, match_stream, stream); - if (!a2dp_stream) - return FALSE; - - if (a2dp_stream->suspend_timer) { - timeout_remove(a2dp_stream->suspend_timer); - a2dp_stream->suspend_timer = 0; - } - - if (!a2dp_stream->suspending) - return TRUE; - - a2dp_stream->suspending = FALSE; - - setup = find_setup_by_session(session); - if (!setup) - return TRUE; - - start = setup->start; - setup->start = FALSE; - - finalize_suspend(setup); - - if (!start) - return TRUE; - - start_err = avdtp_start(session, a2dp_stream->stream); - if (start_err < 0 && start_err != -EINPROGRESS) { - error("avdtp_start: %s (%d)", strerror(-start_err), - -start_err); - finalize_setup_errno(setup, start_err, finalize_resume, NULL); - } - - return TRUE; -} - -static void suspend_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_stream *a2dp_stream; - struct a2dp_setup *setup; - gboolean start; - int start_err; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Suspend_Cfm", sep); - else - DBG("Source %p: Suspend_Cfm", sep); - - a2dp_stream = queue_find(a2dp_sep->streams, match_stream, stream); - if (!a2dp_stream) - return; - - a2dp_stream->suspending = FALSE; - - setup = find_setup_by_session(session); - if (!setup) - return; - - start = setup->start; - setup->start = FALSE; - - if (err) { - setup->stream = NULL; - setup_error_set(setup, err); - } - - finalize_suspend(setup); - - if (!start) - return; - - if (err) { - finalize_resume(setup); - return; - } - - start_err = avdtp_start(session, a2dp_stream->stream); - if (start_err < 0 && start_err != -EINPROGRESS) { - error("avdtp_start: %s (%d)", strerror(-start_err), -start_err); - finalize_setup_errno(setup, start_err, finalize_suspend, NULL); - } -} - -static gboolean close_ind(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Close_Ind", sep); - else - DBG("Source %p: Close_Ind", sep); - - setup = find_setup_by_session(session); - if (!setup) - return TRUE; - - finalize_setup_errno(setup, -ECONNRESET, finalize_suspend, - finalize_resume, NULL); - - return TRUE; -} - -static struct a2dp_remote_sep *find_remote_sep(struct a2dp_channel *chan, - struct a2dp_sep *sep) -{ - struct avdtp_remote_sep *rsep; - - if (!chan || !sep) - return NULL; - - rsep = avdtp_find_remote_sep(chan->session, sep->lsep); - - return queue_find(chan->seps, match_remote_sep, rsep); -} - -static gboolean a2dp_reconfigure(gpointer data) -{ - struct a2dp_setup *setup = data; - struct a2dp_sep *sep = setup->sep; - int posix_err; - struct avdtp_media_codec_capability *rsep_codec; - struct avdtp_service_capability *cap; - - setup->id = 0; - - if (setup->err) { - posix_err = error_to_errno(setup->err); - goto failed; - } - - if (!sep->lsep) { - error("no valid local SEP"); - posix_err = -EINVAL; - goto failed; - } - - if (setup->rsep) { - cap = avdtp_get_codec(setup->rsep->sep); - rsep_codec = (struct avdtp_media_codec_capability *) cap->data; - /* Check that codec really match after closing */ - if (sep->codec != rsep_codec->media_codec_type) - setup->rsep = NULL; - } - - if (!setup->rsep) - setup->rsep = find_remote_sep(setup->chan, sep); - - if (!setup->rsep) { - error("unable to find remote SEP"); - posix_err = -EINVAL; - goto failed; - } - - posix_err = avdtp_set_configuration(setup->session, setup->rsep->sep, - sep->lsep, - setup->caps, - &setup->stream); - if (posix_err < 0) { - error("avdtp_set_configuration: %s", strerror(-posix_err)); - goto failed; - } - - return FALSE; - -failed: - finalize_setup_errno(setup, posix_err, finalize_config, NULL); - return FALSE; -} - -static bool setup_reconfigure(struct a2dp_setup *setup) -{ - if (!setup->reconfigure || setup->id) - return false; - - DBG("%p", setup); - - setup->id = g_idle_add(a2dp_reconfigure, setup); - - setup->reconfigure = FALSE; - - return true; -} - -static struct a2dp_remote_sep *get_remote_sep(struct a2dp_channel *chan, - struct avdtp_stream *stream) -{ - struct avdtp_remote_sep *rsep; - - rsep = avdtp_stream_get_remote_sep(stream); - - return queue_find(chan->seps, match_remote_sep, rsep); -} - -static void close_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Close_Cfm", sep); - else - DBG("Source %p: Close_Cfm", sep); - - setup = find_setup_by_session(session); - if (!setup) - return; - - if (err) { - setup->stream = NULL; - setup_error_set(setup, err); - finalize_config(setup); - return; - } - - if (!setup->rsep) - setup->rsep = get_remote_sep(setup->chan, stream); - - setup_reconfigure(setup); -} - -static void abort_ind(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Abort_Ind", sep); - else - DBG("Source %p: Abort_Ind", sep); - - a2dp_stream_destroy(a2dp_sep, stream); - - setup = find_setup_by_session(session); - if (!setup) - return; - - finalize_setup_errno(setup, -ECONNRESET, finalize_suspend, - finalize_resume, - finalize_config, NULL); - - return; -} - -static void abort_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: Abort_Cfm", sep); - else - DBG("Source %p: Abort_Cfm", sep); - - setup = find_setup_by_session(session); - if (!setup) - return; - - if (setup_reconfigure(setup)) - return; - - setup_unref(setup); -} - -static gboolean reconf_ind(struct avdtp *session, struct avdtp_local_sep *sep, - uint8_t *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: ReConfigure_Ind", sep); - else - DBG("Source %p: ReConfigure_Ind", sep); - - return TRUE; -} - -static gboolean endpoint_delayreport_ind(struct avdtp *session, - struct avdtp_local_sep *sep, - uint8_t rseid, uint16_t delay, - uint8_t *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: DelayReport_Ind", sep); - else - DBG("Source %p: DelayReport_Ind", sep); - - if (a2dp_sep->endpoint == NULL || - a2dp_sep->endpoint->set_delay == NULL) - return FALSE; - - a2dp_sep->endpoint->set_delay(a2dp_sep, delay, a2dp_sep->user_data); - - return TRUE; -} - -static void reconf_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - struct a2dp_setup *setup; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: ReConfigure_Cfm", sep); - else - DBG("Source %p: ReConfigure_Cfm", sep); - - setup = find_setup_by_session(session); - if (!setup) - return; - - if (err) { - setup->stream = NULL; - setup_error_set(setup, err); - } - - finalize_config(setup); -} - -static void delay_report_cfm(struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data) -{ - struct a2dp_sep *a2dp_sep = user_data; - - if (a2dp_sep->type == AVDTP_SEP_TYPE_SINK) - DBG("Sink %p: DelayReport_Cfm", sep); - else - DBG("Source %p: DelayReport_Cfm", sep); -} - -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, - .delay_report = delay_report_cfm, -}; - -static struct avdtp_sep_ind endpoint_ind = { - .match_codec = endpoint_match_codec_ind, - .get_capability = endpoint_getcap_ind, - .set_configuration = endpoint_setconf_ind, - .get_configuration = getconf_ind, - .open = open_ind, - .start = start_ind, - .suspend = suspend_ind, - .close = close_ind, - .abort = abort_ind, - .reconfigure = reconf_ind, - .delayreport = endpoint_delayreport_ind, -}; - -static sdp_record_t *a2dp_record(uint8_t type) -{ - sdp_list_t *svclass_id, *pfseq, *apseq, *root; - uuid_t root_uuid, l2cap_uuid, avdtp_uuid, a2dp_uuid; - 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; - uint16_t a2dp_ver = 0x0104, avdtp_ver = 0x0103, feat = 0x000f; - - record = sdp_record_alloc(); - if (!record) - return NULL; - - sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); - root = sdp_list_append(0, &root_uuid); - sdp_set_browse_groups(record, root); - - if (type == AVDTP_SEP_TYPE_SOURCE) - sdp_uuid16_create(&a2dp_uuid, AUDIO_SOURCE_SVCLASS_ID); - else - sdp_uuid16_create(&a2dp_uuid, AUDIO_SINK_SVCLASS_ID); - svclass_id = sdp_list_append(0, &a2dp_uuid); - sdp_set_service_classes(record, svclass_id); - - sdp_uuid16_create(&profile[0].uuid, ADVANCED_AUDIO_PROFILE_ID); - profile[0].version = a2dp_ver; - pfseq = sdp_list_append(0, &profile[0]); - sdp_set_profile_descs(record, pfseq); - - sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID); - proto[0] = sdp_list_append(0, &l2cap_uuid); - 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_uuid, AVDTP_UUID); - proto[1] = sdp_list_append(0, &avdtp_uuid); - version = sdp_data_alloc(SDP_UINT16, &avdtp_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); - - if (type == AVDTP_SEP_TYPE_SOURCE) - sdp_set_info_attr(record, "Audio Source", 0, 0); - else - sdp_set_info_attr(record, "Audio Sink", 0, 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); - - return record; -} - -static struct a2dp_server *find_server(GSList *list, struct btd_adapter *a) -{ - - for (; list; list = list->next) { - struct a2dp_server *server = list->data; - - if (server->adapter == a) - return server; - } - - return NULL; -} - -static void remote_sep_free(void *data) -{ - struct a2dp_remote_sep *sep = data; - - avdtp_remote_sep_set_destroy(sep->sep, NULL, NULL); - - free(sep->path); - free(sep); -} - -static void remove_remote_sep(void *data) -{ - struct a2dp_remote_sep *sep = data; - - if (!sep->path) { - remote_sep_free(sep); - return; - } - - g_dbus_unregister_interface(btd_get_dbus_connection(), sep->path, - MEDIA_ENDPOINT_INTERFACE); -} - -static void channel_free(void *data) -{ - struct a2dp_channel *chan = data; - struct a2dp_setup *setup; - - if (chan->auth_id > 0) - btd_cancel_authorization(chan->auth_id); - - if (chan->io_id > 0) - g_source_remove(chan->io_id); - - if (chan->io) { - g_io_channel_shutdown(chan->io, TRUE, NULL); - g_io_channel_unref(chan->io); - } - - avdtp_remove_state_cb(chan->state_id); - - queue_destroy(chan->seps, remove_remote_sep); - free(chan->last_used); - - setup = find_setup_by_session(chan->session); - if (setup) { - setup->chan = NULL; - setup_ref(setup); - /* Finalize pending commands before we NULL setup->session */ - finalize_setup_errno(setup, -ENOTCONN, finalize_all, NULL); - avdtp_unref(setup->session); - setup->session = NULL; - setup_unref(setup); - } - - g_free(chan); -} - -static void channel_remove(struct a2dp_channel *chan) -{ - struct a2dp_server *server = chan->server; - - DBG("chan %p", chan); - - queue_remove(server->channels, chan); - - channel_free(chan); -} - -static gboolean disconnect_cb(GIOChannel *io, GIOCondition cond, gpointer data) -{ - struct a2dp_channel *chan = data; - - DBG("chan %p", chan); - - chan->io_id = 0; - - channel_remove(chan); - - return FALSE; -} - -static void caps_add_codec(GSList **l, uint8_t codec, uint8_t *caps, - size_t size) -{ - struct avdtp_service_capability *media_transport, *media_codec; - struct avdtp_media_codec_capability *cap; - - media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT, - NULL, 0); - - *l = g_slist_append(*l, media_transport); - - cap = g_malloc0(sizeof(*cap) + size); - cap->media_type = AVDTP_MEDIA_TYPE_AUDIO; - cap->media_codec_type = codec; - memcpy(cap->data, caps, size); - - media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, cap, - sizeof(*cap) + size); - - *l = g_slist_append(*l, media_codec); - g_free(cap); -} - -struct client { - const char *sender; - const char *path; -}; - -static int match_client(const void *data, const void *user_data) -{ - struct a2dp_sep *sep = (void *) data; - const struct a2dp_endpoint *endpoint = sep->endpoint; - const struct client *client = user_data; - - if (strcmp(client->sender, endpoint->get_name(sep, sep->user_data))) - return -1; - - return strcmp(client->path, endpoint->get_path(sep, sep->user_data)); -} - -static struct a2dp_sep *find_sep(struct a2dp_server *server, uint8_t type, - const char *sender, const char *path) -{ - GSList *l; - struct client client = { sender, path }; - - l = type == AVDTP_SEP_TYPE_SINK ? server->sources : server->sinks; - - l = g_slist_find_custom(l, &client, match_client); - if (l) - return l->data; - - return NULL; -} - -static int parse_properties(DBusMessageIter *props, uint8_t **caps, int *size) -{ - while (dbus_message_iter_get_arg_type(props) == DBUS_TYPE_DICT_ENTRY) { - const char *key; - DBusMessageIter value, entry; - int var; - - dbus_message_iter_recurse(props, &entry); - dbus_message_iter_get_basic(&entry, &key); - - dbus_message_iter_next(&entry); - dbus_message_iter_recurse(&entry, &value); - - var = dbus_message_iter_get_arg_type(&value); - if (strcasecmp(key, "Capabilities") == 0) { - DBusMessageIter array; - - if (var != DBUS_TYPE_ARRAY) - return -EINVAL; - - dbus_message_iter_recurse(&value, &array); - dbus_message_iter_get_fixed_array(&array, caps, size); - return 0; - } - - dbus_message_iter_next(props); - } - - return -EINVAL; -} - -static void reconfig_cb(struct avdtp *session, struct a2dp_sep *sep, - struct avdtp_stream *stream, int err, void *user_data) -{ - DBusMessage *msg = user_data; - - if (err) - g_dbus_send_message(btd_get_dbus_connection(), - btd_error_failed(msg, strerror(-err))); - else - g_dbus_send_reply(btd_get_dbus_connection(), msg, - DBUS_TYPE_INVALID); - - dbus_message_unref(msg); -} - -static int a2dp_reconfig(struct a2dp_channel *chan, const char *sender, - struct a2dp_sep *lsep, struct a2dp_remote_sep *rsep, - uint8_t *caps, int size, void *user_data) -{ - struct a2dp_setup *setup; - struct a2dp_stream *stream; - struct a2dp_setup_cb *cb_data; - GSList *l; - int err; - - if (!chan->session) - return -ENOTCONN; - - setup = a2dp_setup_get(chan->session); - if (!setup) - return -ENOMEM; - - cb_data = setup_cb_add(setup); - cb_data->config_cb = reconfig_cb; - cb_data->user_data = user_data; - - setup->sep = lsep; - setup->rsep = rsep; - - g_slist_free_full(setup->caps, g_free); - setup->caps = NULL; - - caps_add_codec(&setup->caps, setup->sep->codec, caps, size); - - l = avdtp_get_type(rsep->sep) == AVDTP_SEP_TYPE_SINK ? - chan->server->sources : - chan->server->sinks; - - /* Check for existing stream and close it */ - for (; l; l = g_slist_next(l)) { - struct a2dp_sep *tmp = l->data; - - /* Attempt to reconfigure if a stream already exists */ - stream = queue_find(tmp->streams, match_stream_session, - chan->session); - if (stream) { - /* Only allow switching sep from the same sender */ - if (strcmp(sender, tmp->endpoint->get_name(tmp, - tmp->user_data))) { - err = -EPERM; - goto fail; - } - - /* Check if stream is for the channel */ - if (!avdtp_has_stream(chan->session, stream->stream)) - continue; - - err = avdtp_close(chan->session, stream->stream, FALSE); - if (err < 0) { - err = avdtp_abort(chan->session, - stream->stream); - if (err < 0) { - error("avdtp_abort: %s", - strerror(-err)); - goto fail; - } - } - - setup->reconfigure = TRUE; - - return 0; - } - } - - err = avdtp_set_configuration(setup->session, setup->rsep->sep, - lsep->lsep, - setup->caps, - &setup->stream); - if (err < 0) { - error("avdtp_set_configuration: %s", strerror(-err)); - goto fail; - } - - return 0; - -fail: - setup_cb_free(cb_data); - return err; -} - -static DBusMessage *set_configuration(DBusConnection *conn, DBusMessage *msg, - void *data) -{ - struct a2dp_remote_sep *rsep = data; - struct a2dp_channel *chan = rsep->chan; - struct a2dp_sep *lsep = NULL; - struct avdtp_service_capability *service; - struct avdtp_media_codec_capability *codec; - DBusMessageIter args, props; - const char *sender, *path; - uint8_t *caps; - int err, size = 0; - - sender = dbus_message_get_sender(msg); - - dbus_message_iter_init(msg, &args); - - dbus_message_iter_get_basic(&args, &path); - dbus_message_iter_next(&args); - - lsep = find_sep(chan->server, avdtp_get_type(rsep->sep), sender, path); - if (!lsep) - return btd_error_invalid_args(msg); - - service = avdtp_get_codec(rsep->sep); - codec = (struct avdtp_media_codec_capability *) service->data; - - /* Check if codec really matches */ - if (!endpoint_match_codec_ind(chan->session, codec, lsep)) - return btd_error_invalid_args(msg); - - dbus_message_iter_recurse(&args, &props); - if (dbus_message_iter_get_arg_type(&props) != DBUS_TYPE_DICT_ENTRY) - return btd_error_invalid_args(msg); - - if (parse_properties(&props, &caps, &size) < 0) - return btd_error_invalid_args(msg); - - err = a2dp_reconfig(chan, sender, lsep, rsep, caps, size, - dbus_message_ref(msg)); - if (err < 0) { - dbus_message_unref(msg); - return btd_error_failed(msg, strerror(-err)); - } - - return NULL; -} - -static const GDBusMethodTable sep_methods[] = { - { GDBUS_ASYNC_METHOD("SetConfiguration", - GDBUS_ARGS({ "endpoint", "o" }, - { "properties", "a{sv}" } ), - NULL, set_configuration) }, - { }, -}; - -static gboolean get_uuid(const GDBusPropertyTable *property, - DBusMessageIter *iter, void *data) -{ - struct a2dp_remote_sep *sep = data; - const char *uuid; - - switch (avdtp_get_type(sep->sep)) { - case AVDTP_SEP_TYPE_SOURCE: - uuid = A2DP_SOURCE_UUID; - break; - case AVDTP_SEP_TYPE_SINK: - uuid = A2DP_SINK_UUID; - break; - default: - uuid = ""; - } - - dbus_message_iter_append_basic(iter, DBUS_TYPE_STRING, &uuid); - - return TRUE; -} - -static gboolean get_codec(const GDBusPropertyTable *property, - DBusMessageIter *iter, void *data) -{ - struct a2dp_remote_sep *sep = data; - struct avdtp_service_capability *cap = avdtp_get_codec(sep->sep); - struct avdtp_media_codec_capability *codec = (void *) cap->data; - - dbus_message_iter_append_basic(iter, DBUS_TYPE_BYTE, - &codec->media_codec_type); - - return TRUE; -} - -static gboolean get_capabilities(const GDBusPropertyTable *property, - DBusMessageIter *iter, void *data) -{ - struct a2dp_remote_sep *sep = data; - struct avdtp_service_capability *service = avdtp_get_codec(sep->sep); - struct avdtp_media_codec_capability *codec = (void *) service->data; - uint8_t *caps = codec->data; - DBusMessageIter array; - - dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, - DBUS_TYPE_BYTE_AS_STRING, &array); - - dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE, &caps, - service->length - sizeof(*codec)); - - dbus_message_iter_close_container(iter, &array); - - return TRUE; -} - -static gboolean get_device(const GDBusPropertyTable *property, - DBusMessageIter *iter, void *data) -{ - struct a2dp_remote_sep *sep = data; - const char *path; - - path = device_get_path(sep->chan->device); - - dbus_message_iter_append_basic(iter, DBUS_TYPE_OBJECT_PATH, &path); - - return TRUE; -} - -static gboolean get_delay_reporting(const GDBusPropertyTable *property, - DBusMessageIter *iter, void *data) -{ - struct a2dp_remote_sep *sep = data; - dbus_bool_t delay_report; - - delay_report = avdtp_get_delay_reporting(sep->sep); - - dbus_message_iter_append_basic(iter, DBUS_TYPE_BOOLEAN, &delay_report); - - return TRUE; -} - -static const GDBusPropertyTable sep_properties[] = { - { "UUID", "s", get_uuid, NULL, NULL }, - { "Codec", "y", get_codec, NULL, NULL }, - { "Capabilities", "ay", get_capabilities, NULL, NULL }, - { "Device", "o", get_device, NULL, NULL }, - { "DelayReporting", "b", get_delay_reporting, NULL, NULL }, - { } -}; - -static void remote_sep_destroy(void *user_data) -{ - struct a2dp_remote_sep *sep = user_data; - - if (queue_remove(sep->chan->seps, sep)) - remove_remote_sep(sep); -} - -static struct a2dp_remote_sep *register_remote_sep(void *data, void *user_data) -{ - struct avdtp_remote_sep *rsep = data; - struct a2dp_channel *chan = user_data; - struct a2dp_remote_sep *sep; - - sep = queue_find(chan->seps, match_remote_sep, rsep); - if (sep) - return sep; - - if (!avdtp_get_codec(rsep)) { - error("Unable to get remote sep codec"); - return NULL; - } - - sep = new0(struct a2dp_remote_sep, 1); - sep->chan = chan; - sep->sep = rsep; - - if (asprintf(&sep->path, "%s/sep%d", - device_get_path(chan->device), - avdtp_get_seid(rsep)) < 0) { - error("Could not allocate path for remote sep %s/sep%d", - device_get_path(chan->device), - avdtp_get_seid(rsep)); - sep->path = NULL; - goto done; - } - - if (g_dbus_register_interface(btd_get_dbus_connection(), - sep->path, MEDIA_ENDPOINT_INTERFACE, - sep_methods, NULL, sep_properties, - sep, remote_sep_free) == FALSE) { - error("Could not register remote sep %s", sep->path); - free(sep->path); - free(sep); - return NULL; - } - - DBG("Found remote SEP: %s", sep->path); - - avdtp_remote_sep_set_destroy(rsep, sep, remote_sep_destroy); - -done: - queue_push_tail(chan->seps, sep); - - return sep; -} - -static bool match_seid(const void *data, const void *user_data) -{ - const struct a2dp_remote_sep *sep = data; - const uint8_t *seid = user_data; - - return avdtp_get_seid(sep->sep) == *seid; -} - -static int match_sep(const void *data, const void *user_data) -{ - struct a2dp_sep *sep = (void *) data; - const uint8_t *seid = user_data; - - return *seid - avdtp_sep_get_seid(sep->lsep); -} - -static struct a2dp_sep *find_sep_by_seid(struct a2dp_server *server, - uint8_t seid) -{ - GSList *l; - - l = g_slist_find_custom(server->sources, &seid, match_sep); - if (l) - return l->data; - - l = g_slist_find_custom(server->sinks, &seid, match_sep); - if (l) - return l->data; - - return NULL; -} - -static void load_remote_sep(struct a2dp_channel *chan, GKeyFile *key_file, - char **seids) -{ - struct a2dp_sep *lsep; - struct a2dp_remote_sep *sep; - struct avdtp_remote_sep *rsep; - uint8_t lseid, rseid; - char *value; - bool update = false; - - if (!seids) - return; - - for (; *seids; seids++) { - uint8_t type; - uint8_t codec; - uint8_t delay_reporting; - GSList *l = NULL; - char caps[256]; - uint8_t data[128]; - int i, size; - - if (sscanf(*seids, "%02hhx", &rseid) != 1) - continue; - - value = g_key_file_get_string(key_file, "Endpoints", *seids, - NULL); - if (!value) - continue; - - /* Try loading with delay_reporting first */ - if (sscanf(value, "%02hhx:%02hhx:%02hhx:%s", &type, &codec, - &delay_reporting, caps) != 4) { - /* Try old format */ - if (sscanf(value, "%02hhx:%02hhx:%s", &type, &codec, - caps) != 3) { - warn("Unable to load Endpoint: seid %u", rseid); - g_free(value); - continue; - } - delay_reporting = false; - } - - for (i = 0, size = strlen(caps); i < size; i += 2) { - uint8_t *tmp = data + i / 2; - - if (sscanf(caps + i, "%02hhx", tmp) != 1) { - warn("Unable to load Endpoint: seid %u", rseid); - break; - } - } - - g_free(value); - - if (i != size) - continue; - - caps_add_codec(&l, codec, data, size / 2); - - rsep = avdtp_register_remote_sep(chan->session, rseid, type, l, - delay_reporting); - if (!rsep) { - warn("Unable to register Endpoint: seid %u", rseid); - continue; - } - - sep = register_remote_sep(rsep, chan); - if (!sep) { - avdtp_unregister_remote_sep(chan->session, rsep); - update = true; - continue; - } - - sep->from_cache = true; - } - - /* Update cache */ - if (update) - store_remote_seps(chan); - - value = g_key_file_get_string(key_file, "Endpoints", "LastUsed", NULL); - if (!value) - return; - - if (sscanf(value, "%02hhx:%02hhx", &lseid, &rseid) != 2) { - warn("Unable to load LastUsed"); - g_free(value); - return; - } - - g_free(value); - - lsep = find_sep_by_seid(chan->server, lseid); - if (!lsep) { - warn("Unable to load LastUsed: lseid %u not found", lseid); - return; - } - - sep = queue_find(chan->seps, match_seid, &rseid); - if (!sep) { - warn("Unable to load LastUsed: rseid %u not found", rseid); - return; - } - - DBG("LastUsed: lseid %u rseid %u", lseid, rseid); - - add_last_used(chan, lsep, sep); -} - -static void load_remote_seps(struct a2dp_channel *chan) -{ - struct btd_device *device = chan->device; - char filename[PATH_MAX]; - char dst_addr[18]; - char **keys; - GKeyFile *key_file; - GError *gerr = NULL; - - ba2str(device_get_address(device), dst_addr); - - create_filename(filename, PATH_MAX, "/%s/cache/%s", - btd_adapter_get_storage_dir(device_get_adapter(device)), - dst_addr); - - key_file = g_key_file_new(); - if (!g_key_file_load_from_file(key_file, filename, 0, &gerr)) { - error("Unable to load key file from %s: (%s)", filename, - gerr->message); - g_error_free(gerr); - } - keys = g_key_file_get_keys(key_file, "Endpoints", NULL, NULL); - - load_remote_sep(chan, key_file, keys); - - g_strfreev(keys); - g_key_file_free(key_file); -} - -static void avdtp_state_cb(struct btd_device *dev, struct avdtp *session, - avdtp_session_state_t old_state, - avdtp_session_state_t new_state, - void *user_data) -{ - struct a2dp_channel *chan = user_data; - - switch (new_state) { - case AVDTP_SESSION_STATE_DISCONNECTED: - if (chan->session == session) - channel_remove(chan); - break; - case AVDTP_SESSION_STATE_CONNECTING: - break; - case AVDTP_SESSION_STATE_CONNECTED: - if (!chan->session) - chan->session = session; - load_remote_seps(chan); - break; - } -} - -static struct a2dp_channel *channel_new(struct a2dp_server *server, - struct btd_device *device, - GIOChannel *io) -{ - struct a2dp_channel *chan; - - chan = g_new0(struct a2dp_channel, 1); - chan->server = server; - chan->device = device; - chan->seps = queue_new(); - chan->state_id = avdtp_add_state_cb(device, avdtp_state_cb, chan); - - if (!queue_push_tail(server->channels, chan)) { - g_free(chan); - return NULL; - } - - if (!io) - return chan; - - chan->io = g_io_channel_ref(io); - chan->io_id = g_io_add_watch(io, G_IO_ERR | G_IO_HUP | G_IO_NVAL, - (GIOFunc) disconnect_cb, chan); - - return chan; -} - -static bool match_by_device(const void *data, const void *user_data) -{ - const struct a2dp_channel *chan = data; - const struct btd_device *device = user_data; - - return chan->device == device; -} - -struct avdtp *a2dp_avdtp_get(struct btd_device *device) -{ - struct a2dp_server *server; - struct a2dp_channel *chan; - - server = find_server(servers, device_get_adapter(device)); - if (server == NULL) - return NULL; - - chan = queue_find(server->channels, match_by_device, device); - if (!chan) { - chan = channel_new(server, device, NULL); - if (!chan) - return NULL; - } - - if (chan->session) - return avdtp_ref(chan->session); - - /* Check if there is any SEP available */ - if (!queue_isempty(server->seps)) - goto found; - - DBG("Unable to find any available SEP"); - - return NULL; - -found: - chan->session = avdtp_new(chan->io, device, server->seps); - if (!chan->session) { - channel_remove(chan); - return NULL; - } - - return avdtp_ref(chan->session); -} - -static void connect_cb(GIOChannel *io, GError *err, gpointer user_data) -{ - struct a2dp_channel *chan = user_data; - - if (err) { - error("%s", err->message); - goto fail; - } - - if (!chan->session) { - chan->session = avdtp_new(chan->io, chan->device, - chan->server->seps); - if (!chan->session) { - error("Unable to create AVDTP session"); - goto fail; - } - } - - g_io_channel_unref(chan->io); - chan->io = NULL; - - g_source_remove(chan->io_id); - chan->io_id = 0; - - return; - -fail: - channel_remove(chan); -} - -static void auth_cb(DBusError *derr, void *user_data) -{ - struct a2dp_channel *chan = user_data; - GError *err = NULL; - - chan->auth_id = 0; - - if (derr && dbus_error_is_set(derr)) { - error("Access denied: %s", derr->message); - goto fail; - } - - if (!bt_io_accept(chan->io, connect_cb, chan, NULL, &err)) { - error("bt_io_accept: %s", err->message); - g_error_free(err); - goto fail; - } - - return; - -fail: - channel_remove(chan); -} - -static void transport_cb(GIOChannel *io, GError *err, gpointer user_data) -{ - struct a2dp_setup *setup = user_data; - uint16_t omtu, imtu; - - if (!g_slist_find(setups, setup)) { - warn("bt_io_accept: setup %p no longer valid", setup); - g_io_channel_shutdown(io, TRUE, NULL); - return; - } - - if (err) { - error("%s", err->message); - goto drop; - } - - bt_io_get(io, &err, BT_IO_OPT_OMTU, &omtu, BT_IO_OPT_IMTU, &imtu, - BT_IO_OPT_INVALID); - if (err) { - error("%s", err->message); - g_error_free(err); - goto drop; - } - - if (!avdtp_stream_set_transport(setup->stream, - g_io_channel_unix_get_fd(io), - imtu, omtu)) - goto drop; - - g_io_channel_set_close_on_unref(io, FALSE); - - g_io_channel_unref(setup->io); - setup->io = NULL; - - setup_unref(setup); - - return; - -drop: - setup_unref(setup); - g_io_channel_shutdown(io, TRUE, NULL); - -} -static void confirm_cb(GIOChannel *io, gpointer data) -{ - struct a2dp_server *server = data; - struct a2dp_channel *chan; - char address[18]; - bdaddr_t src, dst; - GError *err = NULL; - struct btd_device *device; - - bt_io_get(io, &err, - BT_IO_OPT_SOURCE_BDADDR, &src, - BT_IO_OPT_DEST_BDADDR, &dst, - BT_IO_OPT_DEST, address, - BT_IO_OPT_INVALID); - if (err) { - error("%s", err->message); - g_error_free(err); - goto drop; - } - - DBG("AVDTP: incoming connect from %s", address); - - device = btd_adapter_find_device(adapter_find(&src), &dst, - BDADDR_BREDR); - if (!device) - goto drop; - - chan = queue_find(server->channels, match_by_device, device); - if (chan) { - struct a2dp_setup *setup; - - setup = find_setup_by_session(chan->session); - if (!setup || !setup->stream) - goto drop; - - if (setup->io || avdtp_stream_get_transport(setup->stream, - NULL, NULL, NULL, NULL)) { - error("transport channel already exists"); - goto drop; - } - - if (!bt_io_accept(io, transport_cb, setup, NULL, &err)) { - error("bt_io_accept: %s", err->message); - g_error_free(err); - goto drop; - } - - /* - * Reference the channel so it can be shutdown properly - * stopping bt_io_accept from calling the callback with invalid - * setup pointer. - */ - setup->io = g_io_channel_ref(io); - - return; - } - - chan = channel_new(server, device, io); - if (!chan) - goto drop; - - chan->auth_id = btd_request_authorization(&src, &dst, - ADVANCED_AUDIO_UUID, - auth_cb, chan); - if (chan->auth_id == 0 && !chan->session) - channel_remove(chan); - - return; - -drop: - g_io_channel_shutdown(io, TRUE, NULL); -} - -static bool a2dp_server_listen(struct a2dp_server *server) -{ - GError *err = NULL; - BtIOMode mode; - - if (server->io) - return true; - - mode = btd_opts.avdtp.session_mode; - - server->io = bt_io_listen(NULL, confirm_cb, server, NULL, &err, - BT_IO_OPT_SOURCE_BDADDR, - btd_adapter_get_address(server->adapter), - BT_IO_OPT_PSM, AVDTP_PSM, - BT_IO_OPT_MODE, mode, - BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_MEDIUM, - BT_IO_OPT_CENTRAL, true, - /* Set Input MTU to 0 for auto-tune attempt */ - BT_IO_OPT_IMTU, 0, - BT_IO_OPT_INVALID); - if (server->io) - return true; - - error("%s", err->message); - g_error_free(err); - - return false; -} - -static struct a2dp_server *a2dp_server_register(struct btd_adapter *adapter) -{ - struct a2dp_server *server; - - server = g_new0(struct a2dp_server, 1); - - server->adapter = btd_adapter_ref(adapter); - server->seps = queue_new(); - server->channels = queue_new(); - - servers = g_slist_append(servers, server); - - return server; -} - -static void a2dp_unregister_sep(struct a2dp_sep *sep) -{ - struct a2dp_server *server = sep->server; - - if (sep->destroy) { - sep->destroy(sep->user_data); - sep->endpoint = NULL; - } - - avdtp_unregister_sep(server->seps, &server->seid_pool, sep->lsep); - - queue_destroy(sep->streams, a2dp_stream_free); - g_free(sep); - - if (!queue_isempty(server->seps)) - return; - - if (server->io) { - g_io_channel_shutdown(server->io, TRUE, NULL); - g_io_channel_unref(server->io); - server->io = NULL; - } -} - -static void a2dp_server_unregister(struct a2dp_server *server) -{ - servers = g_slist_remove(servers, server); - queue_destroy(server->channels, channel_free); - queue_destroy(server->seps, NULL); - - if (server->io) { - g_io_channel_shutdown(server->io, TRUE, NULL); - g_io_channel_unref(server->io); - } - - btd_adapter_unref(server->adapter); - g_free(server); -} - -struct a2dp_sep *a2dp_add_sep(struct btd_adapter *adapter, uint8_t type, - uint8_t codec, gboolean delay_reporting, - struct a2dp_endpoint *endpoint, - void *user_data, GDestroyNotify destroy, - int *err) -{ - struct a2dp_server *server; - struct a2dp_sep *sep; - GSList **l; - uint32_t *record_id; - sdp_record_t *record; - - server = find_server(servers, adapter); - if (server == NULL) { - if (err) - *err = -EPROTONOSUPPORT; - return NULL; - } - - if (type == AVDTP_SEP_TYPE_SINK && !server->sink_enabled) { - if (err) - *err = -EPROTONOSUPPORT; - return NULL; - } - - if (type == AVDTP_SEP_TYPE_SOURCE && !server->source_enabled) { - if (err) - *err = -EPROTONOSUPPORT; - return NULL; - } - - sep = g_new0(struct a2dp_sep, 1); - - sep->lsep = avdtp_register_sep(server->seps, &server->seid_pool, type, - AVDTP_MEDIA_TYPE_AUDIO, codec, - delay_reporting, &endpoint_ind, - &cfm, sep); - - if (sep->lsep == NULL) { - g_free(sep); - if (err) - *err = -EINVAL; - return NULL; - } - - sep->server = server; - sep->endpoint = endpoint; - sep->codec = codec; - sep->type = type; - sep->delay_reporting = delay_reporting; - - if (type == AVDTP_SEP_TYPE_SOURCE) { - l = &server->sources; - record_id = &server->source_record_id; - } else { - l = &server->sinks; - record_id = &server->sink_record_id; - } - - if (*record_id != 0) - goto add; - - record = a2dp_record(type); - if (!record) { - error("Unable to allocate new service record"); - a2dp_unregister_sep(sep); - if (err) - *err = -EINVAL; - return NULL; - } - - if (adapter_service_add(server->adapter, record) < 0) { - error("Unable to register A2DP service record"); - sdp_record_free(record); - a2dp_unregister_sep(sep); - if (err) - *err = -EINVAL; - return NULL; - } - - if (!a2dp_server_listen(server)) { - sdp_record_free(record); - a2dp_unregister_sep(sep); - if (err) - *err = -EINVAL; - return NULL; - } - - *record_id = record->handle; - -add: - *l = g_slist_append(*l, sep); - - sep->user_data = user_data; - sep->destroy = destroy; - - if (err) - *err = 0; - return sep; -} - -static bool match_locked(const void *data, const void *user_data) -{ - const struct a2dp_stream *stream = data; - - return stream->locked; -} - -void a2dp_remove_sep(struct a2dp_sep *sep) -{ - struct a2dp_server *server = sep->server; - - if (sep->type == AVDTP_SEP_TYPE_SOURCE) { - if (g_slist_find(server->sources, sep) == NULL) - return; - server->sources = g_slist_remove(server->sources, sep); - if (server->sources == NULL && server->source_record_id) { - adapter_service_remove(server->adapter, - server->source_record_id); - server->source_record_id = 0; - } - } else { - if (g_slist_find(server->sinks, sep) == NULL) - return; - server->sinks = g_slist_remove(server->sinks, sep); - if (server->sinks == NULL && server->sink_record_id) { - adapter_service_remove(server->adapter, - server->sink_record_id); - server->sink_record_id = 0; - } - } - - if (queue_find(sep->streams, match_locked, NULL)) - return; - - a2dp_unregister_sep(sep); -} - -static void select_cb(struct a2dp_setup *setup, void *ret, int size) -{ - struct avdtp_service_capability *service; - struct avdtp_media_codec_capability *codec; - int err; - - if (setup->err) - goto done; - - if (size >= 0) { - caps_add_codec(&setup->caps, setup->sep->codec, ret, size); - goto done; - } - - setup->sep = queue_pop_head(setup->eps); - if (!setup->sep) { - error("Unable to select a valid configuration"); - goto done; - } - - setup->rsep = find_remote_sep(setup->chan, setup->sep); - service = avdtp_get_codec(setup->rsep->sep); - codec = (struct avdtp_media_codec_capability *) service->data; - - err = setup->sep->endpoint->select_configuration(setup->sep, - codec->data, - service->length - sizeof(*codec), - setup, - select_cb, setup->sep->user_data); - if (err == 0) - return; - -done: - finalize_select(setup); - setup_unref(setup); -} - -static struct queue *a2dp_find_eps(struct avdtp *session, GSList *list, - const char *sender) -{ - struct a2dp_channel *chan = find_channel(session); - struct queue *seps = NULL; - - for (; list; list = list->next) { - struct a2dp_sep *sep = list->data; - struct avdtp_remote_sep *rsep; - - /* Use sender's endpoint if available */ - if (sender) { - const char *name; - - if (sep->endpoint == NULL) - continue; - - name = sep->endpoint->get_name(sep, sep->user_data); - if (g_strcmp0(sender, name) != 0) - continue; - } - - rsep = avdtp_find_remote_sep(session, sep->lsep); - if (!rsep) - continue; - - if (!seps) - seps = queue_new(); - - /* Prepend last used so it is preferred over others */ - if (chan->last_used && (chan->last_used->lsep == sep && - chan->last_used->rsep->sep == rsep)) - queue_push_head(seps, sep); - else - queue_push_tail(seps, sep); - } - - return seps; -} - -static struct queue *a2dp_select_eps(struct avdtp *session, uint8_t type, - const char *sender) -{ - struct a2dp_server *server; - struct queue *seps; - GSList *l; - - server = find_server(servers, avdtp_get_adapter(session)); - if (!server) - return NULL; - - l = type == AVDTP_SEP_TYPE_SINK ? server->sources : server->sinks; - - /* Check sender's seps first */ - seps = a2dp_find_eps(session, l, sender); - if (seps != NULL) - return seps; - - return a2dp_find_eps(session, l, NULL); -} - -static void foreach_register_remote_sep(void *data, void *user_data) -{ - register_remote_sep(data, user_data); -} - -static void discover_cb(struct avdtp *session, GSList *seps, - struct avdtp_error *err, void *user_data) -{ - struct a2dp_setup *setup = user_data; - uint16_t version = avdtp_get_version(session); - - DBG("version 0x%04x err %p", version, err); - - setup->seps = seps; - if (err) - setup_error_set(setup, err); - - if (!err) { - g_slist_foreach(seps, foreach_register_remote_sep, setup->chan); - - /* Only store version has been initialized as features like - * Delay Reporting may not be queried if the version in - * unknown. - */ - if (version) - store_remote_seps(setup->chan); - } - - finalize_discover(setup); -} - -unsigned int a2dp_discover(struct avdtp *session, a2dp_discover_cb_t cb, - void *user_data) -{ - struct a2dp_setup *setup; - struct a2dp_setup_cb *cb_data; - - setup = a2dp_setup_get(session); - if (!setup) - return 0; - - /* Don't add cb since avdtp_discover can end up disconnecting the - * session causing the cb to be freed. - */ - cb_data = setup_cb_new(setup); - cb_data->discover_cb = cb; - cb_data->user_data = user_data; - - if (avdtp_discover(session, discover_cb, setup) == 0) { - setup->cb = g_slist_append(setup->cb, cb_data); - return cb_data->id; - } - - setup_cb_free(cb_data); - return 0; -} - -unsigned int a2dp_select_capabilities(struct avdtp *session, - uint8_t type, const char *sender, - a2dp_select_cb_t cb, - void *user_data) -{ - struct a2dp_setup *setup; - struct a2dp_setup_cb *cb_data; - struct queue *eps; - struct avdtp_service_capability *service; - struct avdtp_media_codec_capability *codec; - int err; - - eps = a2dp_select_eps(session, type, sender); - if (!eps) { - error("Unable to select SEP"); - return 0; - } - - setup = a2dp_setup_get(session); - if (!setup) - return 0; - - cb_data = setup_cb_add(setup); - cb_data->select_cb = cb; - cb_data->user_data = user_data; - - setup->eps = eps; - setup->sep = queue_pop_head(eps); - setup->rsep = find_remote_sep(setup->chan, setup->sep); - - if (setup->rsep == NULL) { - error("Could not find remote sep"); - goto fail; - } - - service = avdtp_get_codec(setup->rsep->sep); - codec = (struct avdtp_media_codec_capability *) service->data; - - err = setup->sep->endpoint->select_configuration(setup->sep, - codec->data, - service->length - - sizeof(*codec), - setup_ref(setup), - select_cb, - setup->sep->user_data); - if (err == 0) - return cb_data->id; - - setup_unref(setup); - -fail: - setup_cb_free(cb_data); - return 0; - -} - -unsigned int a2dp_config(struct avdtp *session, struct a2dp_sep *sep, - a2dp_config_cb_t cb, GSList *caps, - void *user_data) -{ - struct a2dp_setup_cb *cb_data; - GSList *l; - struct a2dp_server *server; - struct a2dp_setup *setup; - struct a2dp_stream *stream; - struct avdtp_service_capability *cap; - struct avdtp_media_codec_capability *codec_cap = NULL; - int posix_err; - - server = find_server(servers, avdtp_get_adapter(session)); - if (!server) - return 0; - - for (l = caps; l != NULL; l = l->next) { - cap = l->data; - - if (cap->category != AVDTP_MEDIA_CODEC) - continue; - - codec_cap = (void *) cap->data; - break; - } - - if (!codec_cap) - return 0; - - if (sep->codec != codec_cap->media_codec_type) - return 0; - - DBG("a2dp_config: selected SEP %p", sep->lsep); - - setup = a2dp_setup_get(session); - if (!setup) - return 0; - - cb_data = setup_cb_add(setup); - cb_data->config_cb = cb; - cb_data->user_data = user_data; - - stream = queue_find(sep->streams, match_stream_session, session); - - setup->sep = sep; - setup->stream = stream ? stream->stream : NULL; - - /* Copy given caps if they are different than current caps */ - if (setup->caps != caps) { - g_slist_free_full(setup->caps, g_free); - setup->caps = g_slist_copy(caps); - } - - switch (avdtp_stream_get_state(setup->stream)) { - case AVDTP_STATE_IDLE: - if (sep->type == AVDTP_SEP_TYPE_SOURCE) - l = server->sources; - else - l = server->sinks; - - for (; l != NULL; l = l->next) { - struct a2dp_sep *tmp = l->data; - - stream = queue_find(tmp->streams, match_stream_session, - session); - if (!stream) - continue; - - if (avdtp_has_stream(session, stream->stream)) - break; - } - - if (l != NULL) { - if (stream->locked) - goto failed; - setup->reconfigure = TRUE; - if (avdtp_close(session, stream->stream, FALSE) < 0) { - error("avdtp_close failed"); - goto failed; - } - break; - } - - setup->rsep = find_remote_sep(setup->chan, sep); - if (setup->rsep == NULL) { - error("No matching ACP and INT SEPs found"); - goto failed; - } - - posix_err = avdtp_set_configuration(session, setup->rsep->sep, - sep->lsep, caps, - &setup->stream); - if (posix_err < 0) { - error("avdtp_set_configuration: %s", - strerror(-posix_err)); - goto failed; - } - break; - case AVDTP_STATE_OPEN: - case AVDTP_STATE_STREAMING: - if (avdtp_stream_has_capabilities(setup->stream, caps)) { - DBG("Configuration match: resuming"); - cb_data->source_id = g_idle_add(finalize_config, - setup); - } else if (!setup->reconfigure) { - setup->reconfigure = TRUE; - if (avdtp_close(session, setup->stream, FALSE) < 0) { - error("avdtp_close failed"); - goto failed; - } - } - break; - case AVDTP_STATE_CONFIGURED: - case AVDTP_STATE_CLOSING: - case AVDTP_STATE_ABORTING: - default: - error("SEP in bad state for requesting a new stream"); - goto failed; - } - - return cb_data->id; - -failed: - setup_cb_free(cb_data); - return 0; -} - -unsigned int a2dp_resume(struct avdtp *session, struct a2dp_sep *sep, - a2dp_stream_cb_t cb, void *user_data) -{ - struct a2dp_setup_cb *cb_data; - struct a2dp_setup *setup; - struct a2dp_stream *stream; - - setup = a2dp_setup_get(session); - if (!setup) - return 0; - - cb_data = setup_cb_add(setup); - cb_data->resume_cb = cb; - cb_data->user_data = user_data; - - if (setup->reconfigure) - goto failed; - - stream = queue_find(sep->streams, match_stream_session, session); - - setup->sep = sep; - setup->stream = stream ? stream->stream : NULL; - - switch (avdtp_stream_get_state(setup->stream)) { - case AVDTP_STATE_IDLE: - goto failed; - case AVDTP_STATE_CONFIGURED: - setup->start = TRUE; - break; - case AVDTP_STATE_OPEN: - if (avdtp_start(session, setup->stream) < 0) { - error("avdtp_start failed"); - goto failed; - } - stream->starting = TRUE; - break; - case AVDTP_STATE_STREAMING: - if (!stream->suspending && stream->suspend_timer) { - timeout_remove(stream->suspend_timer); - stream->suspend_timer = 0; - } - if (stream->suspending) - setup->start = TRUE; - else - cb_data->source_id = g_idle_add(finalize_resume, - setup); - break; - case AVDTP_STATE_CLOSING: - case AVDTP_STATE_ABORTING: - default: - error("SEP in bad state for resume"); - goto failed; - } - - return cb_data->id; - -failed: - setup_cb_free(cb_data); - return 0; -} - -unsigned int a2dp_suspend(struct avdtp *session, struct a2dp_sep *sep, - a2dp_stream_cb_t cb, void *user_data) -{ - struct a2dp_setup_cb *cb_data; - struct a2dp_setup *setup; - struct a2dp_stream *stream; - - setup = a2dp_setup_get(session); - if (!setup) - return 0; - - cb_data = setup_cb_add(setup); - cb_data->suspend_cb = cb; - cb_data->user_data = user_data; - - if (setup->reconfigure) - goto failed; - - stream = queue_find(sep->streams, match_stream_session, session); - - setup->sep = sep; - setup->stream = stream ? stream->stream : NULL; - - switch (avdtp_stream_get_state(setup->stream)) { - case AVDTP_STATE_IDLE: - error("a2dp_suspend: no stream to suspend"); - goto failed; - break; - case AVDTP_STATE_OPEN: - cb_data->source_id = g_idle_add(finalize_suspend, setup); - break; - case AVDTP_STATE_STREAMING: - if (avdtp_suspend(session, setup->stream) < 0) { - error("avdtp_suspend failed"); - goto failed; - } - stream->suspending = TRUE; - break; - case AVDTP_STATE_CONFIGURED: - case AVDTP_STATE_CLOSING: - case AVDTP_STATE_ABORTING: - default: - error("SEP in bad state for suspend"); - goto failed; - } - - return cb_data->id; - -failed: - setup_cb_free(cb_data); - return 0; -} - -gboolean a2dp_cancel(unsigned int id) -{ - GSList *ls; - - for (ls = setups; ls != NULL; ls = g_slist_next(ls)) { - struct a2dp_setup *setup = ls->data; - GSList *l; - for (l = setup->cb; l != NULL; l = g_slist_next(l)) { - struct a2dp_setup_cb *cb = l->data; - - if (cb->id != id) - continue; - - setup_ref(setup); - setup_cb_free(cb); - - if (!setup->cb) { - DBG("aborting setup %p", setup); - if (!avdtp_abort(setup->session, setup->stream)) - return TRUE; - } - - setup_unref(setup); - return TRUE; - } - } - - return FALSE; -} - -gboolean a2dp_sep_lock(struct a2dp_sep *sep, struct avdtp *session) -{ - struct a2dp_stream *stream; - - stream = queue_find(sep->streams, match_stream_session, session); - if (!stream || stream->locked) - return FALSE; - - DBG("stream %p locked", sep->lsep); - stream->locked = TRUE; - - return TRUE; -} - -gboolean a2dp_sep_unlock(struct a2dp_sep *sep, struct avdtp *session) -{ - struct a2dp_server *server = sep->server; - struct a2dp_stream *stream; - avdtp_state_t state; - GSList *l; - - stream = queue_find(sep->streams, match_stream_session, session); - if (!stream) - return FALSE; - - state = avdtp_stream_get_state(stream->stream); - - stream->locked = FALSE; - - DBG("stream %p unlocked", stream); - - if (sep->type == AVDTP_SEP_TYPE_SOURCE) - l = server->sources; - else - l = server->sinks; - - /* Unregister sep if it was removed */ - if (g_slist_find(l, sep) == NULL) { - a2dp_unregister_sep(sep); - return TRUE; - } - - if (!stream->stream || state == AVDTP_STATE_IDLE) - return TRUE; - - switch (state) { - case AVDTP_STATE_OPEN: - /* Set timer here */ - break; - case AVDTP_STATE_STREAMING: - if (avdtp_suspend(session, stream->stream) == 0) - stream->suspending = TRUE; - break; - case AVDTP_STATE_IDLE: - case AVDTP_STATE_CONFIGURED: - case AVDTP_STATE_CLOSING: - case AVDTP_STATE_ABORTING: - default: - break; - } - - return TRUE; -} - -struct avdtp_stream *a2dp_sep_get_stream(struct a2dp_sep *sep, - struct avdtp *session) -{ - struct a2dp_stream *stream; - - stream = queue_find(sep->streams, match_stream_session, session); - if (stream) - return stream->stream; - - return NULL; -} - -struct btd_device *a2dp_setup_get_device(struct a2dp_setup *setup) -{ - if (setup->session == NULL) - return NULL; - - return avdtp_get_device(setup->session); -} - -const char *a2dp_setup_remote_path(struct a2dp_setup *setup) -{ - if (setup->rsep) { - return setup->rsep->path; - } - - return NULL; -} - -static int a2dp_source_probe(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - - DBG("path %s", device_get_path(dev)); - - source_init(service); - - return 0; -} - -static void a2dp_source_remove(struct btd_service *service) -{ - source_unregister(service); -} - -static int a2dp_sink_probe(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - - DBG("path %s", device_get_path(dev)); - - return sink_init(service); -} - -static void a2dp_sink_remove(struct btd_service *service) -{ - sink_unregister(service); -} - -static int a2dp_source_connect(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - struct btd_adapter *adapter = device_get_adapter(dev); - struct a2dp_server *server; - const char *path = device_get_path(dev); - - DBG("path %s", path); - - server = find_server(servers, adapter); - if (!server || !server->sink_enabled) { - DBG("Unexpected error: cannot find server"); - return -EPROTONOSUPPORT; - } - - /* Return protocol not available if no record/endpoint exists */ - if (server->sink_record_id == 0) - return -ENOPROTOOPT; - - return source_connect(service); -} - -static int a2dp_source_disconnect(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - const char *path = device_get_path(dev); - - DBG("path %s", path); - - return source_disconnect(service); -} - -static int a2dp_sink_connect(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - struct btd_adapter *adapter = device_get_adapter(dev); - struct a2dp_server *server; - const char *path = device_get_path(dev); - - DBG("path %s", path); - - server = find_server(servers, adapter); - if (!server || !server->source_enabled) { - DBG("Unexpected error: cannot find server"); - return -EPROTONOSUPPORT; - } - - /* Return protocol not available if no record/endpoint exists */ - if (server->source_record_id == 0) - return -ENOPROTOOPT; - - return sink_connect(service); -} - -static int a2dp_sink_disconnect(struct btd_service *service) -{ - struct btd_device *dev = btd_service_get_device(service); - const char *path = device_get_path(dev); - - DBG("path %s", path); - - return sink_disconnect(service); -} - -static int a2dp_source_server_probe(struct btd_profile *p, - struct btd_adapter *adapter) -{ - struct a2dp_server *server; - - DBG("path %s", adapter_get_path(adapter)); - - server = find_server(servers, adapter); - if (server != NULL) - goto done; - - server = a2dp_server_register(adapter); - if (server == NULL) - return -EPROTONOSUPPORT; - -done: - server->source_enabled = TRUE; - - return 0; -} - -static void a2dp_source_server_remove(struct btd_profile *p, - struct btd_adapter *adapter) -{ - struct a2dp_server *server; - - DBG("path %s", adapter_get_path(adapter)); - - server = find_server(servers, adapter); - if (!server) - return; - - g_slist_free_full(server->sources, - (GDestroyNotify) a2dp_unregister_sep); - - if (server->source_record_id) { - adapter_service_remove(server->adapter, - server->source_record_id); - server->source_record_id = 0; - } - - if (server->sink_record_id) - return; - - a2dp_server_unregister(server); -} - -static int a2dp_sink_server_probe(struct btd_profile *p, - struct btd_adapter *adapter) -{ - struct a2dp_server *server; - - DBG("path %s", adapter_get_path(adapter)); - - server = find_server(servers, adapter); - if (server != NULL) - goto done; - - server = a2dp_server_register(adapter); - if (server == NULL) - return -EPROTONOSUPPORT; - -done: - server->sink_enabled = TRUE; - - return 0; -} - -static void a2dp_sink_server_remove(struct btd_profile *p, - struct btd_adapter *adapter) -{ - struct a2dp_server *server; - - DBG("path %s", adapter_get_path(adapter)); - - server = find_server(servers, adapter); - if (!server) - return; - - g_slist_free_full(server->sinks, (GDestroyNotify) a2dp_unregister_sep); - - if (server->sink_record_id) { - adapter_service_remove(server->adapter, server->sink_record_id); - server->sink_record_id = 0; - } - - if (server->source_record_id) - return; - - a2dp_server_unregister(server); -} - -static int media_server_probe(struct btd_adapter *adapter) -{ - DBG("path %s", adapter_get_path(adapter)); - - return media_register(adapter); -} - -static void media_server_remove(struct btd_adapter *adapter) -{ - DBG("path %s", adapter_get_path(adapter)); - - media_unregister(adapter); -} - -static struct btd_profile a2dp_source_profile = { - .name = "a2dp-source", - .priority = BTD_PROFILE_PRIORITY_MEDIUM, - .bearer = BTD_PROFILE_BEARER_BREDR, - .remote_uuid = A2DP_SOURCE_UUID, - - .device_probe = a2dp_source_probe, - .device_remove = a2dp_source_remove, - - .auto_connect = true, - .connect = a2dp_source_connect, - .disconnect = a2dp_source_disconnect, - - .adapter_probe = a2dp_sink_server_probe, - .adapter_remove = a2dp_sink_server_remove, - - /* Connect source after sink, to prefer sink when conflicting */ - .after_services = BTD_PROFILE_UUID_CB(NULL, A2DP_SINK_UUID), -}; - -static struct btd_profile a2dp_sink_profile = { - .name = "a2dp-sink", - .priority = BTD_PROFILE_PRIORITY_MEDIUM, - .bearer = BTD_PROFILE_BEARER_BREDR, - .remote_uuid = A2DP_SINK_UUID, - .device_probe = a2dp_sink_probe, - .device_remove = a2dp_sink_remove, - - .auto_connect = true, - .connect = a2dp_sink_connect, - .disconnect = a2dp_sink_disconnect, - - .adapter_probe = a2dp_source_server_probe, - .adapter_remove = a2dp_source_server_remove, -}; - -static struct btd_adapter_driver media_driver = { - .name = "media", - .probe = media_server_probe, - .remove = media_server_remove, -}; - -static int a2dp_init(void) -{ - btd_register_adapter_driver(&media_driver); - btd_profile_register(&a2dp_source_profile); - btd_profile_register(&a2dp_sink_profile); - - return 0; -} - -static void a2dp_exit(void) -{ - btd_unregister_adapter_driver(&media_driver); - btd_profile_unregister(&a2dp_source_profile); - btd_profile_unregister(&a2dp_sink_profile); -} - -BLUETOOTH_PLUGIN_DEFINE(a2dp, VERSION, BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, - a2dp_init, a2dp_exit) diff --git a/avdtp.h b/avdtp.h deleted file mode 100644 index 10c8f49..0000000 --- a/avdtp.h +++ /dev/null @@ -1,314 +0,0 @@ -/* SPDX-License-Identifier: GPL-2.0-or-later */ -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2006-2010 Nokia Corporation - * Copyright (C) 2004-2010 Marcel Holtmann - * - * - */ - -typedef enum { - AVDTP_SESSION_STATE_DISCONNECTED, - AVDTP_SESSION_STATE_CONNECTING, - AVDTP_SESSION_STATE_CONNECTED -} avdtp_session_state_t; - -struct avdtp; -struct avdtp_server; -struct avdtp_stream; -struct avdtp_local_sep; -struct avdtp_remote_sep; -struct avdtp_error { - uint8_t category; - union { - uint8_t error_code; - int posix_errno; - } err; -}; - -/* 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 -#define AVDTP_DELAY_REPORTING 0x08 -#define AVDTP_ERRNO 0xff - -/* 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)); - -#if __BYTE_ORDER == __LITTLE_ENDIAN - -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)); - -#elif __BYTE_ORDER == __BIG_ENDIAN - -struct avdtp_media_codec_capability { - uint8_t media_type:4; - uint8_t rfa0:4; - uint8_t media_codec_type; - uint8_t data[0]; -} __attribute__ ((packed)); - -#else -#error "Unknown byte order" -#endif - -typedef void (*avdtp_session_state_cb) (struct btd_device *dev, - struct avdtp *session, - avdtp_session_state_t old_state, - avdtp_session_state_t new_state, - void *user_data); - -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); - -typedef void (*avdtp_set_configuration_cb) (struct avdtp *session, - struct avdtp_stream *stream, - struct avdtp_error *err); - -/* Callbacks for when a reply is received to a command that we sent */ -struct avdtp_sep_cfm { - void (*set_configuration) (struct avdtp *session, - struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, - void *user_data); - void (*get_configuration) (struct avdtp *session, - struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, - void *user_data); - void (*open) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data); - void (*start) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, struct avdtp_error *err, - void *user_data); - void (*suspend) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data); - void (*close) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data); - void (*abort) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data); - void (*reconfigure) (struct avdtp *session, - struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data); - void (*delay_report) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - struct avdtp_error *err, void *user_data); -}; - -/* 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 (*match_codec) (struct avdtp *session, - struct avdtp_media_codec_capability *codec, - void *user_data); - gboolean (*get_capability) (struct avdtp *session, - struct avdtp_local_sep *sep, - gboolean get_all, - GSList **caps, uint8_t *err, - void *user_data); - gboolean (*set_configuration) (struct avdtp *session, - struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, - GSList *caps, - avdtp_set_configuration_cb cb, - void *user_data); - gboolean (*get_configuration) (struct avdtp *session, - struct avdtp_local_sep *lsep, - uint8_t *err, void *user_data); - gboolean (*open) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data); - gboolean (*start) (struct avdtp *session, struct avdtp_local_sep *lsep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data); - gboolean (*suspend) (struct avdtp *session, - struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data); - gboolean (*close) (struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data); - void (*abort) (struct avdtp *session, struct avdtp_local_sep *sep, - struct avdtp_stream *stream, uint8_t *err, - void *user_data); - gboolean (*reconfigure) (struct avdtp *session, - struct avdtp_local_sep *lsep, - uint8_t *err, void *user_data); - gboolean (*delayreport) (struct avdtp *session, - struct avdtp_local_sep *lsep, - uint8_t rseid, uint16_t delay, - uint8_t *err, void *user_data); -}; - -typedef void (*avdtp_discover_cb_t) (struct avdtp *session, GSList *seps, - struct avdtp_error *err, void *user_data); - -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_remote_sep *avdtp_register_remote_sep(struct avdtp *session, - uint8_t seid, - uint8_t type, - GSList *caps, - bool delay_reporting); -int avdtp_unregister_remote_sep(struct avdtp *session, - struct avdtp_remote_sep *rsep); - -typedef void (*avdtp_remote_sep_destroy_t)(void *user_data); - -void avdtp_remote_sep_set_destroy(struct avdtp_remote_sep *sep, void *user_data, - avdtp_remote_sep_destroy_t destroy); - -uint8_t avdtp_get_seid(struct avdtp_remote_sep *sep); - -uint8_t avdtp_get_type(struct avdtp_remote_sep *sep); - -struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep); - -bool avdtp_get_delay_reporting(struct avdtp_remote_sep *sep); - -int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb, - void *user_data); - -gboolean avdtp_has_stream(struct avdtp *session, struct avdtp_stream *stream); - -unsigned int avdtp_stream_add_cb(struct avdtp *session, - struct avdtp_stream *stream, - avdtp_stream_state_cb cb, void *data); -gboolean avdtp_stream_remove_cb(struct avdtp *session, - struct avdtp_stream *stream, - unsigned int id); - -gboolean avdtp_stream_set_transport(struct avdtp_stream *stream, int fd, - size_t imtu, size_t omtu); -gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock, - uint16_t *imtu, uint16_t *omtu, - GSList **caps); -struct avdtp_service_capability *avdtp_stream_get_codec( - struct avdtp_stream *stream); -gboolean avdtp_stream_has_capabilities(struct avdtp_stream *stream, - GSList *caps); -gboolean avdtp_stream_has_delay_reporting(struct avdtp_stream *stream); -struct avdtp_remote_sep *avdtp_stream_get_remote_sep( - struct avdtp_stream *stream); - -unsigned int avdtp_add_state_cb(struct btd_device *dev, - avdtp_session_state_cb cb, void *user_data); - -gboolean avdtp_remove_state_cb(unsigned int id); - -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_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, - gboolean immediate); -int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream); -int avdtp_delay_report(struct avdtp *session, struct avdtp_stream *stream, - uint16_t delay); - -struct avdtp_local_sep *avdtp_register_sep(struct queue *lseps, - uint64_t *seid_pool, - uint8_t type, - uint8_t media_type, - uint8_t codec_type, - gboolean delay_reporting, - struct avdtp_sep_ind *ind, - struct avdtp_sep_cfm *cfm, - void *user_data); - -/* Find a matching pair of local and remote SEP ID's */ -struct avdtp_remote_sep *avdtp_find_remote_sep(struct avdtp *session, - struct avdtp_local_sep *lsep); - -int avdtp_unregister_sep(struct queue *lseps, uint64_t *seid_pool, - struct avdtp_local_sep *sep); - -avdtp_state_t avdtp_stream_get_state(struct avdtp_stream *stream); -uint8_t avdtp_sep_get_seid(struct avdtp_local_sep *sep); - -void avdtp_error_init(struct avdtp_error *err, uint8_t type, int id); -const char *avdtp_strerror(struct avdtp_error *err); -uint8_t avdtp_error_category(struct avdtp_error *err); -int avdtp_error_error_code(struct avdtp_error *err); -int avdtp_error_posix_errno(struct avdtp_error *err); - -struct btd_adapter *avdtp_get_adapter(struct avdtp *session); -struct btd_device *avdtp_get_device(struct avdtp *session); -struct avdtp_server *avdtp_get_server(struct avdtp_local_sep *lsep); - -struct avdtp *avdtp_new(GIOChannel *chan, struct btd_device *device, - struct queue *lseps); -uint16_t avdtp_get_version(struct avdtp *session); diff --git a/kernel/src/Api/Audio.hpp b/kernel/src/Api/Audio.hpp index 4ea9222..9d480e7 100644 --- a/kernel/src/Api/Audio.hpp +++ b/kernel/src/Api/Audio.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -35,8 +36,15 @@ namespace Montauk { state == Drivers::USB::Bluetooth::A2dp::State::Streaming || state == Drivers::USB::Bluetooth::A2dp::State::Configured) { Drivers::USB::Bluetooth::A2dp::ConfigureStream(sampleRate, channels, bitsPerSample); - Drivers::USB::Bluetooth::A2dp::StartStream(); - return AUDIO_HANDLE_BT; + if (Drivers::USB::Bluetooth::A2dp::StartStream()) { + return AUDIO_HANDLE_BT; + } + // Stream would not start (sink unresponsive / state desync): + // returning the BT handle anyway would make every write fail + // with the app stuck reporting "playing" at 0:00. Fall through + // to the speakers instead. + Kt::KernelLogStream(Kt::WARNING, "Audio") + << "BT A2DP stream failed to start; falling back to HDA"; } } @@ -56,7 +64,7 @@ namespace Montauk { static int64_t Sys_AudioClose(int handle) { if (handle == AUDIO_HANDLE_BT) { - Drivers::USB::Bluetooth::A2dp::StopStream(); + Drivers::USB::Bluetooth::A2dp::StopStream(true); // drop queued tail return 0; } Drivers::Audio::Mixer::Close(handle); @@ -79,7 +87,8 @@ namespace Montauk { case AUDIO_CTL_GET_VOLUME: return Drivers::USB::Bluetooth::A2dp::GetVolume(); case AUDIO_CTL_PAUSE: - if (value) Drivers::USB::Bluetooth::A2dp::StopStream(); + // Pause keeps the queued PCM so resume continues gaplessly. + if (value) Drivers::USB::Bluetooth::A2dp::StopStream(false); else Drivers::USB::Bluetooth::A2dp::StartStream(); return 0; case AUDIO_CTL_GET_OUTPUT: diff --git a/kernel/src/Api/BuildNo.hpp b/kernel/src/Api/BuildNo.hpp index 9aa0514..8100247 100644 --- a/kernel/src/Api/BuildNo.hpp +++ b/kernel/src/Api/BuildNo.hpp @@ -12,4 +12,4 @@ #pragma once -#define MONTAUK_BUILD_NUMBER 3 +#define MONTAUK_BUILD_NUMBER 12 diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp index 67a0e5e..8acda61 100644 --- a/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp @@ -13,6 +13,7 @@ #include #include #include +#include using namespace Kt; @@ -33,6 +34,7 @@ namespace Drivers::USB::Bluetooth::A2dp { constexpr uint8_t AVDTP_CLOSE = 0x08; constexpr uint8_t AVDTP_SUSPEND = 0x09; constexpr uint8_t AVDTP_ABORT = 0x0A; + constexpr uint8_t AVDTP_GET_ALL_CAPABILITIES = 0x0C; // AVDTP 1.3 // AVDTP message types constexpr uint8_t MSG_COMMAND = 0x00; @@ -109,6 +111,29 @@ namespace Drivers::USB::Bluetooth::A2dp { static uint16_t g_seqNum = 0; static uint32_t g_timestamp = 0; + // PCM ring between WriteAudio (producer, syscall context) and PumpMedia + // (consumer, idle-loop/syscall context). Absolute byte counters wrapping + // mod 2^32: fill = head - tail, buffer index = counter & (SIZE - 1). + constexpr uint32_t PCM_RING_SIZE = 128 * 1024; // ~0.68 s at 48 kHz stereo + constexpr uint64_t LEAD_MS = 150; // sink jitter-buffer target + static uint8_t g_pcmRing[PCM_RING_SIZE]; + static std::atomic g_ringHead{0}; // producer: WriteAudio + static std::atomic g_ringTail{0}; // consumer: PumpMedia + static std::atomic g_pumpActive{false}; // single pumper at a time + static uint32_t g_pcmRate = 48000; + static uint64_t g_clockBase = 0; // ms timestamp of the media clock zero + static uint64_t g_sentSamples = 0; // per-channel samples sent since reset + static uint64_t g_lastSendMs = 0; + static uint32_t g_underruns = 0; // ring ran dry while behind schedule + static bool g_inUnderrun = false; + + static void ResetMediaClock() { + g_clockBase = Timekeeping::GetMilliseconds(); + g_sentSamples = 0; + g_lastSendMs = g_clockBase; + g_inUnderrun = false; + } + // Volume static int g_volume = 80; @@ -409,22 +434,40 @@ namespace Drivers::USB::Bluetooth::A2dp { static bool AvdtpStart() { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; - SendAvdtpCommand(AVDTP_START, payload, 1); - if (!WaitAvdtpResponse()) { - KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout"; - return false; - } - if (!AvdtpAccepted()) { - uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0; + for (int attempt = 0; attempt < 2; attempt++) { + SendAvdtpCommand(AVDTP_START, payload, 1); + + if (!WaitAvdtpResponse()) { + // A lost response is recoverable: re-issue once. AVDTP START + // is idempotent enough for this (a sink that DID start answers + // the retry with BAD_STATE, handled below). + KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout" + << (attempt == 0 ? " (retrying)" : ""); + continue; + } + if (AvdtpAccepted()) { + g_state = State::Streaming; + KernelLogStream(OK, "BT-A2DP") << "Streaming started"; + return true; + } + // Reject payload for START: [first failing ACP SEID][error code]. + uint8_t err = (g_avdtpResponseLen >= 4) ? g_avdtpResponseBuf[3] + : (g_avdtpResponseLen >= 3) ? g_avdtpResponseBuf[2] : 0; + if (err == 0x31) { + // BAD_STATE: the sink's stream is ALREADY streaming -- a state + // desync (our earlier SUSPEND was lost, or a START retry after + // the sink accepted the first one). Adopt its view. + g_state = State::Streaming; + KernelLogStream(INFO, "BT-A2DP") + << "AVDTP Start: sink already streaming (BAD_STATE), continuing"; + return true; + } KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start rejected (err=" << base::hex << (uint64_t)err << base::dec << ")"; return false; } - - g_state = State::Streaming; - KernelLogStream(OK, "BT-A2DP") << "Streaming started"; - return true; + return false; } // ========================================================================= @@ -459,10 +502,435 @@ namespace Drivers::USB::Bluetooth::A2dp { // the Open state; the first audio write (StartStream) issues AVDTP_START. // Without this the headset has no media stream and terminates the link // (HCI disconnect reason 0x13), which is the connect/disconnect flapping. - // Called by L2CAP when data arrives on the SDP channel (the query response). - void ProcessSdp(const uint8_t* data, uint16_t len) { - (void)data; - if (len > 0) g_sdpRspReady = true; + // ========================================================================= + // SDP server + // ========================================================================= + // The headset runs its own SDP query against US right after the signaling + // channel comes up (the Bose QC does: it connects PSM 1 inbound and sends a + // ServiceSearchAttributeRequest), looking for the A2DP AudioSource service + // record the A2DP spec requires a source to expose. If that query goes + // unanswered, the sink never service-authorizes the AVDTP media transport + // channel: it answers our transport CONN_REQ with PENDING (status=2, + // "authorization pending") and the final SUCCESS never arrives -- the + // remoteCid=0/connRsp=1 media-setup failure seen on HW even with SCMS-T + // configured. So we serve one record: A2DP AudioSource. + + // SDP-HOST-TEST-BEGIN: the host harness (/tmp/sdptest) extracts the region + // between these markers verbatim and unit-tests it with libc, so keep it + // free of kernel-only dependencies (KernelLogStream and L2cap::SendData + // are stubbed there). + // AudioSource service record: attribute id (uint16 DE) + value, sorted by + // id, all SDP data elements big-endian. + static const uint8_t kSourceRecord[] = { + // 0x0000 ServiceRecordHandle: uint32 0x00010000 + 0x09, 0x00, 0x00, 0x0A, 0x00, 0x01, 0x00, 0x00, + // 0x0001 ServiceClassIDList: DES { UUID16 AudioSource (0x110A) } + 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x0A, + // 0x0004 ProtocolDescriptorList: + // DES { DES { UUID16 L2CAP (0x0100), uint16 PSM 0x0019 }, + // DES { UUID16 AVDTP (0x0019), uint16 version 0x0103 } } + 0x09, 0x00, 0x04, 0x35, 0x10, + 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x19, + 0x35, 0x06, 0x19, 0x00, 0x19, 0x09, 0x01, 0x03, + // 0x0005 BrowseGroupList: DES { UUID16 PublicBrowseRoot (0x1002) } + 0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02, + // 0x0009 BluetoothProfileDescriptorList: + // DES { DES { UUID16 AdvancedAudioDistribution (0x110D), uint16 0x0103 } } + 0x09, 0x00, 0x09, 0x35, 0x08, + 0x35, 0x06, 0x19, 0x11, 0x0D, 0x09, 0x01, 0x03, + // 0x0311 SupportedFeatures: uint16 0x0001 (Player) + 0x09, 0x03, 0x11, 0x09, 0x00, 0x01, + }; + constexpr uint32_t kSourceRecordHandle = 0x00010000; + + // AVRCP Target service record. Bose (CSR/Qualcomm-stack) sinks couple the + // audio path to remote control: the headset acts as AVRCP Controller for + // absolute volume and may gate/delay the media path when the source has no + // Target. Category 2 (amplifier) + AVRCP 1.4 = absolute volume capable. + static const uint8_t kAvrcpRecord[] = { + // 0x0000 ServiceRecordHandle: uint32 0x00010001 + 0x09, 0x00, 0x00, 0x0A, 0x00, 0x01, 0x00, 0x01, + // 0x0001 ServiceClassIDList: DES { UUID16 A/V RemoteControlTarget (0x110C) } + 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x0C, + // 0x0004 ProtocolDescriptorList: + // DES { DES { UUID16 L2CAP (0x0100), uint16 PSM 0x0017 }, + // DES { UUID16 AVCTP (0x0017), uint16 version 0x0103 } } + 0x09, 0x00, 0x04, 0x35, 0x10, + 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x17, + 0x35, 0x06, 0x19, 0x00, 0x17, 0x09, 0x01, 0x03, + // 0x0005 BrowseGroupList: DES { UUID16 PublicBrowseRoot (0x1002) } + 0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02, + // 0x0009 BluetoothProfileDescriptorList: + // DES { DES { UUID16 A/V RemoteControl (0x110E), uint16 0x0104 } } + 0x09, 0x00, 0x09, 0x35, 0x08, + 0x35, 0x06, 0x19, 0x11, 0x0E, 0x09, 0x01, 0x04, + // 0x0311 SupportedFeatures: uint16 0x0002 (category 2: amplifier) + 0x09, 0x03, 0x11, 0x09, 0x00, 0x02, + }; + constexpr uint32_t kAvrcpRecordHandle = 0x00010001; + + struct SdpRecordDef { + uint32_t Handle; + const uint8_t* Rec; + uint8_t RecLen; + const uint16_t* Uuids; // UUIDs the record "contains" for pattern match + uint8_t NumUuids; + }; + static const uint16_t kSourceUuids[] = {0x110A, 0x110D, 0x0019, 0x0100, 0x1002}; + static const uint16_t kAvrcpUuids[] = {0x110C, 0x110E, 0x0017, 0x0100, 0x1002}; + static const SdpRecordDef kSdpRecords[] = { + {kSourceRecordHandle, kSourceRecord, (uint8_t)sizeof(kSourceRecord), kSourceUuids, 5}, + {kAvrcpRecordHandle, kAvrcpRecord, (uint8_t)sizeof(kAvrcpRecord), kAvrcpUuids, 5}, + }; + constexpr int kNumSdpRecords = 2; + + // Parse one SDP data element header at `off`; on success sets the value's + // offset and length (bounds-checked against `len`). + static bool SdpDeHeader(const uint8_t* d, uint32_t len, uint32_t off, + uint32_t* valOff, uint32_t* valLen) { + if (off >= len) return false; + uint32_t vo = off + 1, vl = 0; + switch (d[off] & 0x07) { // size index + case 0: vl = 1; break; + case 1: vl = 2; break; + case 2: vl = 4; break; + case 3: vl = 8; break; + case 4: vl = 16; break; + case 5: if (vo >= len) return false; + vl = d[vo]; vo += 1; break; + case 6: if (vo + 1 >= len) return false; + vl = ((uint32_t)d[vo] << 8) | d[vo + 1]; vo += 2; break; + case 7: if (vo + 3 >= len) return false; + vl = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16) + | ((uint32_t)d[vo + 2] << 8) | d[vo + 3]; vo += 4; break; + } + if (vo + vl > len) return false; + *valOff = vo; *valLen = vl; + return true; + } + + // Collect the UUIDs named in a ServiceSearchPattern (a DES of UUIDs at + // `off`). UUID32/UUID128-on-base values above 16 bits become 0xFFFF + // (match nothing). Returns the number collected. + static uint32_t SdpPatternUuids(const uint8_t* d, uint32_t len, uint32_t off, + uint16_t* out, uint32_t maxOut) { + static const uint8_t kBaseUuid[12] = // Bluetooth base UUID, bytes 4-15 + {0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB}; + + uint32_t po, pl; + if (!SdpDeHeader(d, len, off, &po, &pl)) return 0; + uint32_t end = po + pl; + uint32_t i = po, n = 0; + while (i < end && n < maxOut) { + uint32_t vo, vl; + if (!SdpDeHeader(d, end, i, &vo, &vl)) break; + uint32_t uuid = 0xFFFFFFFF; + if (d[i] == 0x19 && vl == 2) { // UUID16 + uuid = ((uint32_t)d[vo] << 8) | d[vo + 1]; + } else if (d[i] == 0x1A && vl == 4) { // UUID32 + uuid = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16) + | ((uint32_t)d[vo + 2] << 8) | d[vo + 3]; + } else if (d[i] == 0x1C && vl == 16 + && memcmp(&d[vo + 4], kBaseUuid, 12) == 0) { // UUID128 on base + uuid = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16) + | ((uint32_t)d[vo + 2] << 8) | d[vo + 3]; + } + out[n++] = (uuid <= 0xFFFF) ? (uint16_t)uuid : (uint16_t)0xFFFF; + i = vo + vl; + } + return n; + } + + // Lenient (ANY-of) match: strict SDP semantics demand the record contain + // ALL pattern UUIDs, but headsets bundle service+protocol UUIDs in one + // pattern; returning a near-match record beats returning nothing. + static bool SdpRecordMatches(const SdpRecordDef& r, + const uint16_t* uuids, uint32_t n) { + for (uint32_t i = 0; i < n; i++) + for (uint8_t j = 0; j < r.NumUuids; j++) + if (uuids[i] == r.Uuids[j]) return true; + return false; + } + + // Attribute-id ranges requested in an AttributeIDList data element. + struct AttrRange { uint16_t Start; uint16_t End; }; + + // Parse the AttributeIDList DES at `off` into inclusive (start,end) ranges: + // a uint16 element is a single attribute id, a uint32 element packs a + // start/end range. Returns the count; 0 if absent or malformed, which + // callers treat as "all attributes" (lenient beats wrongly rejecting an + // odd but well-meaning query). + static uint32_t SdpAttrRanges(const uint8_t* d, uint32_t len, uint32_t off, + AttrRange* out, uint32_t maxOut) { + if (off >= len || (d[off] & 0xF8) != 0x30) return 0; // not a DES + uint32_t po, pl; + if (!SdpDeHeader(d, len, off, &po, &pl)) return 0; + uint32_t end = po + pl, i = po, n = 0; + while (i < end && n < maxOut) { + uint32_t vo, vl; + if (!SdpDeHeader(d, end, i, &vo, &vl)) break; + if (d[i] == 0x09 && vl == 2) { // uint16: one attribute id + uint16_t id = ((uint16_t)d[vo] << 8) | d[vo + 1]; + out[n].Start = id; out[n].End = id; n++; + } else if (d[i] == 0x0A && vl == 4) { // uint32: id range + out[n].Start = (uint16_t)(((uint16_t)d[vo] << 8) | d[vo + 1]); + out[n].End = (uint16_t)(((uint16_t)d[vo + 2] << 8) | d[vo + 3]); + n++; + } + i = vo + vl; + } + return n; + } + + static bool SdpAttrWanted(uint16_t id, const AttrRange* r, uint32_t n) { + if (n == 0) return true; + for (uint32_t i = 0; i < n; i++) + if (id >= r[i].Start && id <= r[i].End) return true; + return false; + } + + // Copy the (attribute id, value) pairs of `rec` that the request asked for + // into `out`; returns bytes written. Returning ONLY the requested + // attributes matters: the SDP spec requires it, and an embedded peer's + // parser may walk the response expecting exactly what it asked. + static uint16_t SdpFilterRecord(const uint8_t* rec, uint16_t recLen, + const AttrRange* r, uint32_t nr, + uint8_t* out, uint16_t outMax) { + uint16_t n = 0; + uint32_t i = 0; + while (i + 3 <= recLen) { + if (rec[i] != 0x09) break; // attribute id is always uint16 + uint16_t id = (uint16_t)(((uint16_t)rec[i + 1] << 8) | rec[i + 2]); + uint32_t vo, vl; + if (!SdpDeHeader(rec, recLen, i + 3, &vo, &vl)) break; + uint32_t pairLen = (vo - i) + vl; // id element + value element + if (SdpAttrWanted(id, r, nr)) { + if (n + pairLen > outMax) break; + memcpy(&out[n], &rec[i], pairLen); + n = (uint16_t)(n + pairLen); + } + i += pairLen; + } + return n; + } + + // The in-flight attribute response body, served in chunks no larger than + // the request's MaximumAttributeByteCount. The continuation state we hand + // out is {len=2, resume offset} into this buffer. One transaction at a + // time is plenty for a headset peer. + static uint8_t g_sdpSrvBody[384]; + static uint16_t g_sdpSrvBodyLen = 0; + + static void SdpServerSend(uint16_t cid, uint8_t pduId, uint16_t tid, + const uint8_t* params, uint16_t paramLen) { + uint8_t buf[224] = {}; + if (5u + paramLen > sizeof(buf)) return; + buf[0] = pduId; + buf[1] = (uint8_t)(tid >> 8); // SDP is big-endian throughout + buf[2] = (uint8_t)(tid & 0xFF); + buf[3] = (uint8_t)(paramLen >> 8); + buf[4] = (uint8_t)(paramLen & 0xFF); + memcpy(&buf[5], params, paramLen); + L2cap::SendData(cid, buf, (uint16_t)(5 + paramLen)); + } + + // Answer one SDP request PDU. Runs nested under PollEvents (the ACL rx + // path), which is fine: sending is safe there, only blocking waits are not. + static void SdpHandleRequest(uint16_t cid, const uint8_t* d, uint16_t len) { + if (len < 5) return; + uint8_t pdu = d[0]; + uint16_t tid = ((uint16_t)d[1] << 8) | d[2]; + uint8_t params[200] = {}; + uint16_t n = 0; + + // Pattern match (search PDUs only) against every record we serve. + uint16_t pat[8] = {}; + uint32_t numPat = (pdu == 0x02 || pdu == 0x06) + ? SdpPatternUuids(d, len, 5, pat, 8) : 0; + bool match[kNumSdpRecords] = {}; + uint32_t numMatch = 0; + for (int r = 0; r < kNumSdpRecords; r++) { + match[r] = SdpRecordMatches(kSdpRecords[r], pat, numPat); + if (match[r]) numMatch++; + } + + if (pdu == 0x06 || pdu == 0x04) { + // ServiceSearchAttributeRequest -> 0x07 / ServiceAttributeRequest + // -> 0x05. Both carry a MaximumAttributeByteCount, an + // AttributeIDList, and a continuation state, and both MUST be + // honored: return ONLY the requested attributes and never more + // bytes per response than the peer allowed, continuing across + // requests otherwise. The old server ignored all three and dumped + // every attribute of every record in one oversized response -- a + // spec violation an embedded sink's SDP client may choke on + // silently (its device interrogation then never completes, and a + // stack that service-authorizes channels against that + // interrogation parks them at "authorization pending" forever). + { // Raw request dump: ONE decisive run beats guessing what the + // headset asked for. Queries are once-per-connection rare. + KernelLogStream cl(INFO, "BT-A2DP"); + cl << "SDP req:" << base::hex; + for (uint16_t i = 0; i < len && i < 28; i++) + cl << " " << (uint64_t)d[i]; + cl << base::dec; + } + + // Field offsets differ: 0x06 has the search pattern at 5, 0x04 a + // 4-byte record handle at 5. Both follow with max byte count, + // AttributeIDList, continuation state. + uint32_t at = 5; + const SdpRecordDef* attrRec = nullptr; // 0x04's addressed record + if (pdu == 0x06) { + uint32_t po, pl; + if (!SdpDeHeader(d, len, 5, &po, &pl)) return; + at = po + pl; + } else { + uint32_t handle = (len >= 9) + ? (((uint32_t)d[5] << 24) | ((uint32_t)d[6] << 16) + | ((uint32_t)d[7] << 8) | d[8]) : 0; + for (int r = 0; r < kNumSdpRecords; r++) + if (kSdpRecords[r].Handle == handle) attrRec = &kSdpRecords[r]; + if (!attrRec) { + params[n++] = 0x00; params[n++] = 0x02; // invalid record handle + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + at = 9; + } + + uint16_t maxBytes = 0xFFFF; + AttrRange ranges[8]; + uint32_t numRanges = 0; + uint16_t resumeOff = 0; + bool isCont = false; + if (at + 2 <= len) { + maxBytes = (uint16_t)(((uint16_t)d[at] << 8) | d[at + 1]); + at += 2; + numRanges = SdpAttrRanges(d, len, at, ranges, 8); + uint32_t avo, avl; // step past the id list to + if (SdpDeHeader(d, len, at, &avo, &avl)) at = avo + avl; + // Continuation state: 1-byte length + that many opaque bytes. + // Ours is always 2 bytes (the resume offset we handed out). + if (at + 3 <= len && d[at] == 0x02) { + resumeOff = (uint16_t)(((uint16_t)d[at + 1] << 8) | d[at + 2]); + isCont = true; + } + } + if (maxBytes < 7) maxBytes = 7; // spec minimum + + if (!isCont) { + // Fresh request: build the full response body once, then chunk. + // 0x07's body is an outer DES of per-record attribute-list + // DESes; 0x05's body is the single record's attribute list DES. + uint8_t filtered[192]; + g_sdpSrvBodyLen = 0; + if (pdu == 0x06) { + uint16_t inner = 0; + uint16_t flen[kNumSdpRecords] = {}; + uint8_t fbuf[kNumSdpRecords][192]; + for (int r = 0; r < kNumSdpRecords; r++) { + if (!match[r]) continue; + flen[r] = SdpFilterRecord(kSdpRecords[r].Rec, + kSdpRecords[r].RecLen, + ranges, numRanges, + fbuf[r], sizeof(fbuf[r])); + inner = (uint16_t)(inner + 2 + flen[r]); + } + g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35; + g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)inner; + for (int r = 0; r < kNumSdpRecords; r++) { + if (!match[r]) continue; + g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35; + g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)flen[r]; + memcpy(&g_sdpSrvBody[g_sdpSrvBodyLen], fbuf[r], flen[r]); + g_sdpSrvBodyLen = (uint16_t)(g_sdpSrvBodyLen + flen[r]); + } + } else { + uint16_t flen = SdpFilterRecord(attrRec->Rec, attrRec->RecLen, + ranges, numRanges, + filtered, sizeof(filtered)); + g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35; + g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)flen; + memcpy(&g_sdpSrvBody[g_sdpSrvBodyLen], filtered, flen); + g_sdpSrvBodyLen = (uint16_t)(g_sdpSrvBodyLen + flen); + } + } else if (g_sdpSrvBodyLen == 0 || resumeOff >= g_sdpSrvBodyLen) { + params[n++] = 0x00; params[n++] = 0x05; // invalid continuation + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + + uint16_t chunk = (uint16_t)(g_sdpSrvBodyLen - resumeOff); + if (chunk > maxBytes) chunk = maxBytes; + if (chunk > 180) chunk = 180; // stay inside our buffers + bool more = (uint16_t)(resumeOff + chunk) < g_sdpSrvBodyLen; + + { // Log WHICH services + attributes the headset wants -- decisive + // for diagnosing a sink that gates audio on something we lack. + KernelLogStream cl(OK, "BT-A2DP"); + cl << "SDP server: " << (pdu == 0x06 ? "search [" : "attr [") + << base::hex; + if (pdu == 0x06) + for (uint32_t i = 0; i < numPat; i++) + cl << (i ? " " : "") << (uint64_t)pat[i]; + else + cl << (uint64_t)attrRec->Handle; + cl << "] attrs="; + for (uint32_t i = 0; i < numRanges; i++) + cl << (i ? "," : "") << (uint64_t)ranges[i].Start + << "-" << (uint64_t)ranges[i].End; + cl << base::dec << " max=" << (uint64_t)maxBytes + << " -> " << (uint64_t)(pdu == 0x06 ? numMatch : 1) + << " record(s), " << (uint64_t)chunk << "/" + << (uint64_t)g_sdpSrvBodyLen << " B" + << (isCont ? " (cont)" : "") << (more ? " (+more)" : ""); + } + + params[n++] = (uint8_t)(chunk >> 8); // AttributeList(s)ByteCount + params[n++] = (uint8_t)(chunk & 0xFF); + memcpy(¶ms[n], &g_sdpSrvBody[resumeOff], chunk); + n = (uint16_t)(n + chunk); + if (more) { + uint16_t next = (uint16_t)(resumeOff + chunk); + params[n++] = 0x02; // continuation: 2 bytes + params[n++] = (uint8_t)(next >> 8); + params[n++] = (uint8_t)(next & 0xFF); + } else { + params[n++] = 0x00; // no continuation state + } + SdpServerSend(cid, (pdu == 0x06) ? 0x07 : 0x05, tid, params, n); + } else if (pdu == 0x02) { // ServiceSearchRequest -> 0x03 + params[n++] = 0x00; params[n++] = (uint8_t)numMatch; // total count + params[n++] = 0x00; params[n++] = (uint8_t)numMatch; // current count + for (int r = 0; r < kNumSdpRecords; r++) { + if (!match[r]) continue; + params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 24); + params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 16); + params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 8); + params[n++] = (uint8_t)(kSdpRecords[r].Handle & 0xFF); + } + params[n++] = 0x00; + SdpServerSend(cid, 0x03, tid, params, n); + } else { + params[n++] = 0x00; params[n++] = 0x03; // invalid request syntax + SdpServerSend(cid, 0x01, tid, params, n); + } + } + // SDP-HOST-TEST-END + + // Called by L2CAP when data arrives on ANY SDP channel -- ours (the client + // query response) or one the headset opened to us (a request to serve). + // Dispatch by PDU id, not by channel: requests are even (0x02/0x04/0x06), + // responses odd -- so a stale g_sdpCid colliding with a fresh inbound + // channel after reconnect can never swallow a request. + void ProcessSdp(uint16_t localCid, const uint8_t* data, uint16_t len) { + if (len < 1) return; + uint8_t pdu = data[0]; + if (pdu == 0x02 || pdu == 0x04 || pdu == 0x06) { + SdpHandleRequest(localCid, data, len); + return; + } + if (localCid == g_sdpCid) g_sdpRspReady = true; } // Minimal SDP: open PSM 0x0001, send a ServiceSearchAttributeRequest for the @@ -496,17 +964,28 @@ namespace Drivers::USB::Bluetooth::A2dp { }; L2cap::SendData(cid, pdu, sizeof(pdu)); + bool answered = false; uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); - if (g_sdpRspReady) { - KernelLogStream(OK, "BT-A2DP") << "SDP query answered"; - return true; - } + if (g_sdpRspReady) { answered = true; break; } for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } - KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)"; + + // Transaction over: CLOSE the client channel (FreeChannel sends the + // L2CAP Disconnect since the peer acked it). SDP links are + // per-transaction; every stock stack closes them when done, and a + // channel left dangling for the whole session is exactly the kind of + // oddity an embedded peer's connection manager can trip over. + L2cap::FreeChannel(cid); + g_sdpCid = 0; + + if (answered) { + KernelLogStream(OK, "BT-A2DP") << "SDP query answered (channel closed)"; + } else { + KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)"; + } return true; // channel configured + query sent; proceed to AVDTP } @@ -632,9 +1111,12 @@ namespace Drivers::USB::Bluetooth::A2dp { // and abandoned the very connection the sink was authorizing, while a // single held dial (build 44) already proved holding alone is harmless. // Also accept an inbound transport channel (some sinks open it). - // NOTE: the real gate on Bose is Content Protection -- see - // AvdtpSetConfiguration's SCMS-T handling; without it the sink pends - // this channel forever (connRsp=1, remoteCid=0). + // NOTE: Bose service-gates this channel TWICE: SetConfiguration must + // include Content Protection (SCMS-T, see AvdtpSetConfiguration), AND + // the headset's own inbound SDP query for our AudioSource record must + // have been answered (see the SDP server above). Miss either and the + // sink pends this channel forever (connRsp=1 status=2, authorization + // pending) -- the HW failure on builds without the SDP server. g_mediaCid = 0; constexpr uint32_t kMediaWaitMs = 8000; @@ -671,6 +1153,25 @@ namespace Drivers::USB::Bluetooth::A2dp { KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid=" << base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open"; + + // 5. AVRCP control channel (PSM 0x17), best effort, AFTER the stream + // exists -- the order every phone uses. An earlier build dialed it + // right after the signaling channel; the sink answered PENDING and + // never completed it, so the half-open dial just sat in its + // authorization queue ahead of the media channel. Dialing here + // keeps the bring-up clean and still gives the headset its absolute + // volume path; an inbound AVCTP connect is accepted at any time. + { + uint16_t avrcp = L2cap::Connect(L2cap::PSM_AVCTP); + if (avrcp && L2cap::WaitConfigured(avrcp, 1500)) { + KernelLogStream(OK, "BT-A2DP") << "AVRCP control channel ready, cid=" + << base::hex << (uint64_t)avrcp << base::dec; + } else { + KernelLogStream(INFO, "BT-A2DP") << "AVRCP dial not configured (connRsp=" + << base::hex << (uint64_t)L2cap::LastConnRspResult() << base::dec + << ", continuing)"; + } + } return true; } @@ -711,8 +1212,11 @@ namespace Drivers::USB::Bluetooth::A2dp { break; } - case AVDTP_GET_CAPABILITIES: { - // Respond with our SBC capabilities + case AVDTP_GET_CAPABILITIES: + case AVDTP_GET_ALL_CAPABILITIES: { + // Respond with our SBC capabilities. GET_ALL_CAPABILITIES + // (AVDTP 1.3) must be answered too -- our SDP record + // advertises 1.3, and silence to it stalls the peer. uint8_t rsp[10] = {}; rsp[0] = CAT_MEDIA_TRANSPORT; rsp[1] = 0; @@ -724,7 +1228,7 @@ namespace Drivers::USB::Bluetooth::A2dp { rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0) rsp[8] = 2; // Min bitpool rsp[9] = 53; // Max bitpool - SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10); + SendAvdtpResponse(txLabel, signalId, rsp, 10); break; } @@ -751,31 +1255,49 @@ namespace Drivers::USB::Bluetooth::A2dp { case AVDTP_START: { g_state = State::Streaming; + ResetMediaClock(); SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote started streaming"; break; } + // The sink tearing the stream down MUST be visible in the log: + // each of these silently flips g_state, after which every + // WriteAudio returns -1 (app symptom: track frozen at 0:00 + // with no kernel log output at all). case AVDTP_CLOSE: { g_state = State::Idle; SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0); + KernelLogStream(WARNING, "BT-A2DP") << "Remote CLOSED stream"; break; } case AVDTP_SUSPEND: { g_state = State::Open; SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0); + KernelLogStream(WARNING, "BT-A2DP") << "Remote SUSPENDED stream"; break; } case AVDTP_ABORT: { g_state = State::Idle; SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0); + KernelLogStream(WARNING, "BT-A2DP") << "Remote ABORTED stream"; break; } - default: + default: { + // AVDTP General Reject -- silence to an unknown command can + // stall the peer's signaling state machine. + KernelLogStream(INFO, "BT-A2DP") << "Rejecting unhandled AVDTP cmd 0x" + << base::hex << (uint64_t)signalId << base::dec; + uint8_t rej[2] = { + (uint8_t)((txLabel << 4) | (PKT_SINGLE << 2) | MSG_GENERAL_REJECT), + signalId + }; + L2cap::SendData(g_sigCid, rej, 2); break; + } } } } @@ -795,6 +1317,11 @@ namespace Drivers::USB::Bluetooth::A2dp { g_sbcInitialized = true; g_seqNum = 0; g_timestamp = 0; + g_pcmRate = sampleRate ? sampleRate : 48000; + // Fresh stream: drop any queued PCM from the previous one. + g_ringTail.store(g_ringHead.load(std::memory_order_relaxed), + std::memory_order_release); + ResetMediaClock(); KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: " << (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit " @@ -812,63 +1339,114 @@ namespace Drivers::USB::Bluetooth::A2dp { if (g_state == State::Configured) { if (!AvdtpOpen()) return false; } - return AvdtpStart(); + if (!AvdtpStart()) return false; + ResetMediaClock(); + return true; } return (g_state == State::Streaming); } - bool StopStream() { + bool StopStream(bool flushQueued) { if (g_state == State::Streaming) { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; SendAvdtpCommand(AVDTP_SUSPEND, payload, 1); WaitAvdtpResponse(1000); g_state = State::Open; } + if (flushQueued) { + // Closing the stream (track change / app exit): drop the queued + // tail. A pause keeps it so resume continues gaplessly. (A pump + // on another core may send one final stale frame -- harmless.) + g_ringTail.store(g_ringHead.load(std::memory_order_relaxed), + std::memory_order_release); + } return true; } // ========================================================================= - // WriteAudio — encode PCM to SBC and stream over Bluetooth + // Media pipeline: PCM ring buffer + paced feeder // ========================================================================= + // WriteAudio only copies the app's PCM into a ring and returns at once; + // PumpMedia() encodes and sends frames from the ring, paced to the audio + // clock with LEAD_MS of sink-side jitter buffer, gated on ACL TX + // readiness. It runs from the idle-loop event pump and from WriteAudio. + // Before this ring, the only buffering between the app and the air was + // the controller's handful of ACL buffers (~20 ms of audio) -- any stall + // longer than that (RF retransmission burst, app render/decode hiccup, + // scheduler delay) drained it and audibly dropped out. - int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) { - if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) { - return -1; + void PumpMedia() { + if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) return; + bool expected = false; + if (!g_pumpActive.compare_exchange_strong(expected, true, + std::memory_order_acquire)) { + return; // someone else is pumping } uint32_t samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder); - uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; // 16-bit samples - uint32_t sbcFrameSize = Sbc::GetFrameSize(&g_sbcEncoder); + uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; + int16_t framePcm[512]; - // Apply volume scaling to PCM data - // We work on a local copy for volume adjustment - int16_t scaledPcm[512]; // Max ~128 samples * 2 channels = 256 samples - if (bytesPerFrame > sizeof(scaledPcm)) return -1; + while (bytesPerFrame <= sizeof(framePcm) && g_pcmRate != 0) { + if (g_state != State::Streaming) break; // torn down mid-pump - uint32_t consumed = 0; - uint16_t maxOut = Hci::AclMaxPackets(); - if (maxOut == 0) maxOut = 4; // controller ACL buffer credits + uint32_t fill = g_ringHead.load(std::memory_order_acquire) + - g_ringTail.load(std::memory_order_relaxed); + uint64_t now = Timekeeping::GetMilliseconds(); + uint64_t audioMs = g_sentSamples * 1000 / g_pcmRate; + uint64_t elapsed = now - g_clockBase; - while (consumed + bytesPerFrame <= pcmLen) { - // Copy and scale by volume - const int16_t* src = (const int16_t*)(pcmData + consumed); - uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels; - for (uint32_t i = 0; i < numSamples; i++) { - scaledPcm[i] = (int16_t)(((int32_t)src[i] * g_volume) / 100); + if (fill < bytesPerFrame) { + // Ring dry. If the sink's lead is exhausted too, this is an + // audible dropout caused by the app not feeding in time. + if (!g_inUnderrun && g_sentSamples != 0 && elapsed > audioMs) { + g_inUnderrun = true; + g_underruns++; + } + break; + } + if (audioMs >= elapsed + LEAD_MS) break; // sink lead is full + if (!Hci::AclTxReady()) { + // Normal credit pacing most of the time. A credit pool stuck + // for 250+ ms with the USB side fully drained means NOCP + // events were lost -- reset and carry on. + if (now - g_lastSendMs > 250 && Hci::AclTxInFlight() == 0) { + KernelLogStream(WARNING, "BT-A2DP") + << "media credit stall (seq=" << (uint64_t)g_seqNum + << "); resetting credits"; + Hci::AclResetCredits(); + continue; + } + break; + } + // Way behind schedule (app went silent without SUSPEND): rebase + // the clock instead of bursting the whole backlog at the sink. + if (elapsed > audioMs + 1000) { + g_clockBase = now - audioMs; + elapsed = audioMs; } - // Build media packet: RTP-like header (12 bytes) + SBC payload header (1 byte) + SBC frames - uint8_t mediaPkt[256] = {}; + // Pull one frame from the ring (may wrap) and apply volume. + uint32_t tail = g_ringTail.load(std::memory_order_relaxed); + uint32_t idx = tail & (PCM_RING_SIZE - 1); + uint32_t firstPart = PCM_RING_SIZE - idx; + if (firstPart > bytesPerFrame) firstPart = bytesPerFrame; + memcpy(framePcm, &g_pcmRing[idx], firstPart); + memcpy((uint8_t*)framePcm + firstPart, &g_pcmRing[0], + bytesPerFrame - firstPart); + g_ringTail.store(tail + bytesPerFrame, std::memory_order_release); - // Simplified media packet header (AVDTP media packet) - // Byte 0: V=2, P=0, X=0, CC=0 -> 0x80 - // Byte 1: M=0, PT=96 -> 0x60 - // Bytes 2-3: Sequence number - // Bytes 4-7: Timestamp - // Bytes 8-11: SSRC - // Byte 12: SBC payload header (number of SBC frames) - mediaPkt[0] = 0x80; - mediaPkt[1] = 0x60; + uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels; + for (uint32_t i = 0; i < numSamples; i++) { + framePcm[i] = (int16_t)(((int32_t)framePcm[i] * g_volume) / 100); + } + + // Build media packet: RTP-like header (12 bytes) + optional SCMS-T + // content-protection header (1 byte) + SBC payload header (1 byte) + // + SBC frame + uint8_t mediaPkt[256] = {}; + mediaPkt[0] = 0x80; // V=2, P=0, X=0, CC=0 + mediaPkt[1] = 0x60; // M=0, PT=96 mediaPkt[2] = (uint8_t)(g_seqNum >> 8); mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF); mediaPkt[4] = (uint8_t)(g_timestamp >> 24); @@ -876,37 +1454,86 @@ namespace Drivers::USB::Bluetooth::A2dp { mediaPkt[6] = (uint8_t)(g_timestamp >> 8); mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF); mediaPkt[8] = 0; mediaPkt[9] = 0; mediaPkt[10] = 0; mediaPkt[11] = 0x01; // SSRC - mediaPkt[12] = 1; // Number of SBC frames in this packet - // Encode SBC frame - uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, scaledPcm, &mediaPkt[13]); + // When SCMS-T was configured (SetConfiguration cat 0x04), every + // media packet carries a 1-byte CP header BETWEEN the RTP header + // and the SBC payload header (0x00 = copy permitted, as BlueZ). + uint32_t hdr = 12; + if (g_sinkContentProtection) mediaPkt[hdr++] = 0x00; + mediaPkt[hdr++] = 1; // SBC payload header: number of SBC frames - uint32_t totalLen = 13 + encodedSize; - - // Flow control + event pump. WriteAudio runs in syscall context, - // which otherwise never services the xHCI event ring -- so without - // this the controller's ACL credits (Number-Of-Completed-Packets) - // and the RX ring are never processed and the TX ring stalls/overruns - // (-> no audio). Wait for a controller buffer credit, send, then pump - // so the completion + credit are reaped before the next frame. - uint64_t t0 = Timekeeping::GetMilliseconds(); - while (Hci::AclPendingCount() >= maxOut - && Timekeeping::GetMilliseconds() - t0 < 100) { - Xhci::PollEvents(); - Hci::DrainEvents(); - } - - // Send via L2CAP on media channel - L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)totalLen); - Xhci::PollEvents(); - Hci::DrainEvents(); + uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, framePcm, &mediaPkt[hdr]); + L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)(hdr + encodedSize)); g_seqNum++; g_timestamp += samplesPerFrame; - consumed += bytesPerFrame; + g_sentSamples += samplesPerFrame; + g_lastSendMs = now; + g_inUnderrun = false; + + // Bring-up diagnostic: heartbeat every 2048 frames (~5.5 s of + // audio) proving the pipeline is healthy. + if ((g_seqNum & 0x7FF) == 0) { + uint32_t ringMs = (g_pcmRate && g_sbcEncoder.Channels) + ? (uint32_t)((uint64_t)(fill - bytesPerFrame) * 1000 + / ((uint64_t)g_pcmRate * g_sbcEncoder.Channels * 2)) + : 0; + KernelLogStream(INFO, "BT-A2DP") << "media: seq=" + << (uint64_t)g_seqNum + << " ring=" << (uint64_t)ringMs << "ms" + << " lead=" << (uint64_t)(audioMs > elapsed ? audioMs - elapsed : 0) << "ms" + << " inflight=" << (uint64_t)Hci::AclTxInFlight() + << " pending=" << (uint64_t)Hci::AclPendingCount() + << " underruns=" << (uint64_t)g_underruns; + } } - return (int)consumed; + g_pumpActive.store(false, std::memory_order_release); + } + + // ========================================================================= + // WriteAudio — accept PCM into the ring (never blocks) + // ========================================================================= + + int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) { + if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) { + // The app retries every few ms on -1, so rate-limit hard; but the + // FIRST rejection must be visible -- a silently flipped state here + // is otherwise indistinguishable from the app never writing. + static uint32_t rejCount = 0; + rejCount++; + if (rejCount <= 2 || (rejCount & 0x3FF) == 0) { + KernelLogStream(WARNING, "BT-A2DP") << "WriteAudio rejected #" + << (uint64_t)rejCount << ": sbc=" << (uint64_t)(g_sbcInitialized ? 1 : 0) + << " state=" << (uint64_t)(int)g_state + << " mediaCid=" << base::hex << (uint64_t)g_mediaCid << base::dec; + } + return -1; + } + + // Copy into the ring, clamped to free space and aligned to whole + // stereo sample pairs. Returns the bytes accepted; 0 = ring full, + // the app retries on its next loop pass. + uint32_t head = g_ringHead.load(std::memory_order_relaxed); + uint32_t tail = g_ringTail.load(std::memory_order_acquire); + uint32_t freeBytes = PCM_RING_SIZE - (head - tail); + uint32_t n = (pcmLen < freeBytes ? pcmLen : freeBytes) & ~3u; + + uint32_t idx = head & (PCM_RING_SIZE - 1); + uint32_t firstPart = PCM_RING_SIZE - idx; + if (firstPart > n) firstPart = n; + memcpy(&g_pcmRing[idx], pcmData, firstPart); + memcpy(&g_pcmRing[0], pcmData + firstPart, n - firstPart); + g_ringHead.store(head + n, std::memory_order_release); + + // Reap events (NOCP credits, inbound traffic) and feed the link from + // syscall context too, so streaming keeps moving even when no core + // is idle. + Xhci::PollEvents(); + Hci::DrainEvents(); + PumpMedia(); + + return (int)n; } // ========================================================================= diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp index 93d7e6d..6870fec 100644 --- a/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp @@ -38,8 +38,10 @@ namespace Drivers::USB::Bluetooth::A2dp { // Process an AVDTP signaling packet void ProcessAvdtp(const uint8_t* data, uint16_t len); - // Process an SDP response packet (on the SDP L2CAP channel) - void ProcessSdp(const uint8_t* data, uint16_t len); + // Process an SDP packet (on any SDP L2CAP channel). Dispatches by PDU: + // requests (the headset querying OUR services) are answered by the minimal + // SDP server; responses on our outgoing client channel complete DoSdpQuery. + void ProcessSdp(uint16_t localCid, const uint8_t* data, uint16_t len); // Configure a stream for the given PCM parameters bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample); @@ -47,13 +49,21 @@ namespace Drivers::USB::Bluetooth::A2dp { // Start streaming bool StartStream(); - // Stop streaming - bool StopStream(); + // Stop streaming. flushQueued drops any PCM still queued in the media + // ring: pass true when closing the stream (track change / app exit), + // false for a pause so resume continues gaplessly. + bool StopStream(bool flushQueued = true); - // Write PCM audio data to the Bluetooth audio stream - // Returns number of bytes consumed + // Write PCM audio data to the Bluetooth audio stream. Copies into the + // media ring and returns immediately (never blocks); returns the number + // of bytes accepted (0 = ring full, retry later). int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen); + // Encode + send queued PCM, paced to the audio clock and gated on ACL TX + // readiness. Called from the idle-loop event pump and from WriteAudio; + // self-serializing, cheap no-op when not streaming. + void PumpMedia(); + // Get current state State GetState(); diff --git a/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp b/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp new file mode 100644 index 0000000..46ec4d4 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp @@ -0,0 +1,230 @@ +/* + * Avrcp.cpp + * Bluetooth AVRCP -- minimal Target role over AVCTP (PSM 0x0017) + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "Avrcp.hpp" +#include "A2dp.hpp" +#include "L2cap.hpp" +#include +#include +#include + +using namespace Kt; + +namespace Drivers::USB::Bluetooth::Avrcp { + + // ========================================================================= + // Constants + // ========================================================================= + + // AV/C command types (commands) / response codes (responses) + constexpr uint8_t AVC_CTYPE_CONTROL = 0x0; + constexpr uint8_t AVC_CTYPE_STATUS = 0x1; + constexpr uint8_t AVC_CTYPE_NOTIFY = 0x3; + constexpr uint8_t AVC_RSP_NOT_IMPL = 0x8; + constexpr uint8_t AVC_RSP_ACCEPTED = 0x9; + constexpr uint8_t AVC_RSP_REJECTED = 0xA; + constexpr uint8_t AVC_RSP_STABLE = 0xC; + constexpr uint8_t AVC_RSP_INTERIM = 0xF; + + // AV/C opcodes + constexpr uint8_t AVC_OP_VENDOR = 0x00; + constexpr uint8_t AVC_OP_UNIT_INFO = 0x30; + constexpr uint8_t AVC_OP_SUBUNIT_INFO = 0x31; + constexpr uint8_t AVC_OP_PASSTHROUGH = 0x7C; + + // AVRCP vendor-dependent PDUs (BT SIG company id 00 19 58) + constexpr uint8_t PDU_GET_CAPABILITIES = 0x10; + constexpr uint8_t PDU_GET_PLAY_STATUS = 0x30; + constexpr uint8_t PDU_REGISTER_NOTIFY = 0x31; + constexpr uint8_t PDU_SET_ABS_VOLUME = 0x50; + + // AVRCP notification events + constexpr uint8_t EVT_PLAYBACK_STATUS = 0x01; + constexpr uint8_t EVT_TRACK_CHANGED = 0x02; + constexpr uint8_t EVT_VOLUME_CHANGED = 0x0D; + + // ========================================================================= + // Send helpers + // ========================================================================= + + // AVCTP single-packet response: byte0 = transaction<<4 | pktType<<2 | + // C/R(1=response)<<1 | IPID; bytes 1-2 = PID big-endian. + static void SendAvctp(uint16_t cid, uint8_t transaction, uint16_t pid, + bool invalidPid, const uint8_t* avc, uint16_t avcLen) { + uint8_t buf[96] = {}; + if (3u + avcLen > sizeof(buf)) return; + buf[0] = (uint8_t)((transaction << 4) | 0x02 | (invalidPid ? 0x01 : 0)); + buf[1] = (uint8_t)(pid >> 8); + buf[2] = (uint8_t)(pid & 0xFF); + if (avc && avcLen) memcpy(&buf[3], avc, avcLen); + L2cap::SendData(cid, buf, (uint16_t)(3 + avcLen)); + } + + // AVRCP vendor-dependent response frame: + // [rsp][subunit 0x48 panel][opcode 0x00][company 00 19 58][pdu][pkt 0][len BE][params] + static void SendVendorRsp(uint16_t cid, uint8_t transaction, uint8_t rspCode, + uint8_t pdu, const uint8_t* p, uint16_t n) { + uint8_t avc[64] = {}; + if (10u + n > sizeof(avc)) return; + avc[0] = rspCode; + avc[1] = 0x48; // PANEL subunit + avc[2] = AVC_OP_VENDOR; + avc[3] = 0x00; avc[4] = 0x19; avc[5] = 0x58; // BT SIG company id + avc[6] = pdu; + avc[7] = 0x00; // packet type: single + avc[8] = (uint8_t)(n >> 8); + avc[9] = (uint8_t)(n & 0xFF); + if (p && n) memcpy(&avc[10], p, n); + SendAvctp(cid, transaction, 0x110E, false, avc, (uint16_t)(10 + n)); + } + + // ========================================================================= + // ProcessAvctp + // ========================================================================= + + void ProcessAvctp(uint16_t localCid, const uint8_t* d, uint16_t len) { + if (len < 3) return; + + uint8_t transaction = (d[0] >> 4) & 0x0F; + uint8_t pktType = (d[0] >> 2) & 0x03; + bool isCommand = ((d[0] >> 1) & 0x01) == 0; + uint16_t pid = ((uint16_t)d[1] << 8) | d[2]; + + if (!isCommand) return; // a response to something we sent + if (pktType != 0) return; // only single-packet AVCTP supported + + if (pid != 0x110E) { + // Unsupported PID: respond with the IPID bit set, no message body. + SendAvctp(localCid, transaction, pid, true, nullptr, 0); + return; + } + + const uint8_t* avc = d + 3; + uint16_t alen = (uint16_t)(len - 3); + if (alen < 3) return; + uint8_t ctype = avc[0] & 0x0F; + uint8_t opcode = avc[2]; + + switch (opcode) { + case AVC_OP_UNIT_INFO: { + // Fixed-format reply: PANEL subunit, BT SIG company id. + uint8_t rsp[8] = {AVC_RSP_STABLE, 0xFF, AVC_OP_UNIT_INFO, + 0x07, 0x48, 0x00, 0x19, 0x58}; + SendAvctp(localCid, transaction, pid, false, rsp, sizeof(rsp)); + break; + } + + case AVC_OP_SUBUNIT_INFO: { + uint8_t rsp[8] = {AVC_RSP_STABLE, 0xFF, AVC_OP_SUBUNIT_INFO, + 0x07, 0x48, 0xFF, 0xFF, 0xFF}; + SendAvctp(localCid, transaction, pid, false, rsp, sizeof(rsp)); + break; + } + + case AVC_OP_PASSTHROUGH: { + // Accept every pass-through (play/pause/etc.). Echo the frame + // with the response code swapped in. Wiring the operation ids + // to the Music app is a later feature; ACCEPTED keeps the + // controller-side state machine happy. + uint8_t rsp[16] = {}; + uint16_t cp = (alen > sizeof(rsp)) ? (uint16_t)sizeof(rsp) : alen; + memcpy(rsp, avc, cp); + rsp[0] = AVC_RSP_ACCEPTED; + SendAvctp(localCid, transaction, pid, false, rsp, cp); + if (alen >= 4) { + KernelLogStream(INFO, "BT-AVRCP") << "pass-through op=" + << base::hex << (uint64_t)(avc[3] & 0x7F) + << ((avc[3] & 0x80) ? " release" : " press") << base::dec; + } + break; + } + + case AVC_OP_VENDOR: { + // [ctype][subunit][00][company 3][pdu][pkt][len BE][params] + if (alen < 10) break; + if (avc[3] != 0x00 || avc[4] != 0x19 || avc[5] != 0x58) { + uint8_t rsp[8] = {}; + memcpy(rsp, avc, 8); + rsp[0] = AVC_RSP_NOT_IMPL; + SendAvctp(localCid, transaction, pid, false, rsp, 8); + break; + } + uint8_t pdu = avc[6]; + uint16_t plen = ((uint16_t)avc[8] << 8) | avc[9]; + const uint8_t* p = &avc[10]; + if (10u + plen > alen) plen = (uint16_t)(alen - 10); + + if (pdu == PDU_GET_CAPABILITIES && ctype == AVC_CTYPE_STATUS && plen >= 1) { + if (p[0] == 0x02) { // CompanyID list + uint8_t rp[5] = {0x02, 0x01, 0x00, 0x19, 0x58}; + SendVendorRsp(localCid, transaction, AVC_RSP_STABLE, + pdu, rp, sizeof(rp)); + } else if (p[0] == 0x03) { // EventsSupported + uint8_t rp[5] = {0x03, 0x03, EVT_PLAYBACK_STATUS, + EVT_TRACK_CHANGED, EVT_VOLUME_CHANGED}; + SendVendorRsp(localCid, transaction, AVC_RSP_STABLE, + pdu, rp, sizeof(rp)); + } else { + uint8_t err = 0x01; // invalid parameter + SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED, + pdu, &err, 1); + } + } else if (pdu == PDU_GET_PLAY_STATUS && ctype == AVC_CTYPE_STATUS) { + // song length / position unknown (0xFFFFFFFF), status by state + uint8_t rp[9] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + (uint8_t)(A2dp::IsStreaming() ? 0x01 : 0x00)}; + SendVendorRsp(localCid, transaction, AVC_RSP_STABLE, + pdu, rp, sizeof(rp)); + } else if (pdu == PDU_REGISTER_NOTIFY && ctype == AVC_CTYPE_NOTIFY && plen >= 1) { + // INTERIM response with the current value. (A CHANGED + // follow-up on actual change is a later feature.) + if (p[0] == EVT_VOLUME_CHANGED) { + uint8_t rp[2] = {EVT_VOLUME_CHANGED, + (uint8_t)((A2dp::GetVolume() * 127) / 100)}; + SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, + pdu, rp, sizeof(rp)); + } else if (p[0] == EVT_PLAYBACK_STATUS) { + uint8_t rp[2] = {EVT_PLAYBACK_STATUS, + (uint8_t)(A2dp::IsStreaming() ? 0x01 : 0x00)}; + SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, + pdu, rp, sizeof(rp)); + } else if (p[0] == EVT_TRACK_CHANGED) { + uint8_t rp[9] = {EVT_TRACK_CHANGED, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, + pdu, rp, sizeof(rp)); + } else { + uint8_t err = 0x01; + SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED, + pdu, &err, 1); + } + } else if (pdu == PDU_SET_ABS_VOLUME && ctype == AVC_CTYPE_CONTROL && plen >= 1) { + uint8_t vol = p[0] & 0x7F; + A2dp::SetVolume(((int)vol * 100) / 127); + SendVendorRsp(localCid, transaction, AVC_RSP_ACCEPTED, + pdu, &vol, 1); + KernelLogStream(INFO, "BT-AVRCP") << "absolute volume -> " + << (uint64_t)(((int)vol * 100) / 127) << "%"; + } else { + uint8_t err = 0x00; // invalid command + SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED, + pdu, &err, 1); + } + break; + } + + default: { + uint8_t rsp[8] = {}; + uint16_t cp = (alen > sizeof(rsp)) ? (uint16_t)sizeof(rsp) : alen; + memcpy(rsp, avc, cp); + rsp[0] = AVC_RSP_NOT_IMPL; + SendAvctp(localCid, transaction, pid, false, rsp, cp); + break; + } + } + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp b/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp new file mode 100644 index 0000000..6ecc241 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp @@ -0,0 +1,19 @@ +/* + * Avrcp.hpp + * Bluetooth AVRCP (Audio/Video Remote Control Profile) -- minimal Target role + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include + +namespace Drivers::USB::Bluetooth::Avrcp { + + // Process an AVCTP packet arriving on an AVRCP control channel (PSM 0x0017). + // Implements a minimal AVRCP Target: unit/subunit info, pass-through accept, + // GetCapabilities, RegisterNotification (interim), absolute volume. Runs + // from the ACL receive path (DrainEvents, top-level) -- sending is safe + // there, blocking waits are not (none are used). + void ProcessAvctp(uint16_t localCid, const uint8_t* data, uint16_t len); + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp index a1e4aa0..9b5127c 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp @@ -218,15 +218,32 @@ namespace Drivers::USB::Bluetooth { // Set local name Hci::WriteLocalName("MontaukOS"); - // Set class of device: Audio (Major Service: Audio, Major Class: Audio/Video) - // CoD: 0x240404 = Rendering | Audio | Wearable Headset (common for audio devices) - // For a computer acting as audio source: - // 0x200408 = Audio service | Audio/Video class | Portable Audio - Hci::WriteClassOfDevice(0x200408); + // Class of Device. The A2DP spec MANDATES the Capturing service bit + // (0x080000) for a source; Audio (0x200000) is customary. Major/minor + // class: Computer/Laptop (0x010C) -- what we actually are. The old + // value 0x200408 (Audio/Video major class, minor "hands-free device", + // no Capturing bit) presented MontaukOS to the headset as ANOTHER + // HEADSET. A sink's connection manager classifies peers by CoD (it + // arrives in its Connection Request event and is cached at pairing), + // and an A2DP/AVRCP dial from a "hands-free unit" is a credible reason + // for it to park those channels at "authorization pending" forever. + // NOTE: the headset caches this from pairing -- it must FORGET the + // device and re-pair to observe the new class. + Hci::WriteClassOfDevice(0x28010C); // Enable Simple Secure Pairing Hci::WriteSSPMode(1); + // Allow role switch + sniff on new connections. The controller default + // link policy is 0x0000 (deny both). A multipoint headset (Bose QC + // Ultra) that also holds a link to a phone requests a role switch to + // master on our link to avoid a scatternet; with the switch denied, + // some sink firmwares never grant the A2DP media path. Sniff denial + // similarly upsets CSR-derived stacks that sniff idle links. + uint8_t linkPolicy[2] = {0x05, 0x00}; // bit0 role switch, bit2 sniff + Hci::SendCommand(Hci::OP_WRITE_DEFAULT_LP, linkPolicy, 2); + Hci::WaitCommandComplete(Hci::OP_WRITE_DEFAULT_LP); + // Set event mask to receive relevant events. Octet 6 (events 0x31-0x38) // MUST be enabled for Secure Simple Pairing: IO Capability Request // (0x31, bit 48), IO Capability Response (0x32), User Confirmation @@ -261,6 +278,19 @@ namespace Drivers::USB::Bluetooth { CompleteInit(); } + // ========================================================================= + // ServiceEvents — steady-state event pump (idle loop) + // ========================================================================= + + void ServiceEvents() { + if (!g_initialized) return; + if (Xhci::InPollContext()) return; // never nest under PollEvents + Xhci::PollEvents(); + Hci::DrainEvents(); + Hci::ProcessPendingCommands(); + A2dp::PumpMedia(); // feed queued media to the link (no-op when idle) + } + // ========================================================================= // Public queries // ========================================================================= diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp index 238113c..9bcebf7 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp @@ -19,6 +19,14 @@ namespace Drivers::USB::Bluetooth { // once the boot filesystems are up; a no-op if nothing is pending. void ServiceDeferredInit(); + // Steady-state event service: drains the xHCI event ring + Bluetooth ACL + // RX ring and flushes queued HCI replies. Called from the idle loop so + // inbound traffic (the headset's SDP/AVRCP queries, AVDTP commands, ACL + // credits) is processed even when no syscall happens to be pumping a BT + // wait loop. Cheap no-op when the adapter is down; safe from any core + // (PollEvents/DrainEvents/ProcessPendingCommands all self-serialize). + void ServiceEvents(); + // Query adapter state bool IsInitialized(); uint8_t GetSlotId(); diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.cpp b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp index a2e9c8c..d8c074a 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Hci.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp @@ -6,6 +6,7 @@ #include "Hci.hpp" #include "L2cap.hpp" +#include #include #include #include @@ -64,13 +65,21 @@ namespace Drivers::USB::Bluetooth::Hci { // ACL buffer size (from controller) static uint16_t g_aclMaxLen = 0; static uint16_t g_aclMaxNum = 0; - static volatile uint16_t g_aclPendingCount = 0; + // Controller ACL buffer credits in use: incremented per SendAcl, decremented + // ONLY by Number-Of-Completed-Packets events. The bulk OUT USB completion + // must NOT decrement this -- it only means the packet reached the controller, + // not that the controller sent it over the air. (Decrementing on both used + // to zero the count instantly, defeating A2DP flow control entirely.) + // Atomic: incremented from syscall context, decremented from the event path + // on another core. int32 so a stray double-NOCP clamps instead of wrapping. + static std::atomic g_aclPendingCount{0}; // Diagnostic ACL data-path counters: TX submitted, TX completed (bulk OUT // completion), RX received (bulk IN). Used to tell whether L2CAP signaling - // actually flows over ACL after encryption is enabled. - static volatile uint32_t g_aclTxCount = 0; - static volatile uint32_t g_aclTxDoneCount = 0; + // actually flows over ACL after encryption is enabled. tx - txDone is also + // the number of TX DMA ring slots still owned by the xHCI (see AclTxReady). + static std::atomic g_aclTxCount{0}; + static std::atomic g_aclTxDoneCount{0}; static volatile uint32_t g_aclRxCount = 0; // Incremented when an incoming ACL packet is dropped because the RX ring was // full -- a nonzero value means a burst overran the ring (and could have @@ -305,9 +314,11 @@ namespace Drivers::USB::Bluetooth::Hci { Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket); } } else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) { - // Bulk OUT completion — decrement pending count - g_aclTxDoneCount++; - if (g_aclPendingCount > 0) g_aclPendingCount--; + // Bulk OUT completion: the packet was DMA'd to the controller and + // its TX ring slot is free again. Controller buffer credits + // (g_aclPendingCount) are NOT released here -- only the + // Number-Of-Completed-Packets event does that. + g_aclTxDoneCount.fetch_add(1, std::memory_order_relaxed); } } @@ -554,22 +565,52 @@ namespace Drivers::USB::Bluetooth::Hci { uint32_t totalLen = sizeof(AclHeader) + len; - g_aclPendingCount++; - g_aclTxCount++; + g_aclPendingCount.fetch_add(1, std::memory_order_relaxed); + g_aclTxCount.fetch_add(1, std::memory_order_relaxed); Xhci::QueueBulkOutTransfer(g_slotId, txBuf, txPhys, totalLen); return true; } - uint16_t AclPendingCount() { return g_aclPendingCount; } + uint16_t AclPendingCount() { + int32_t v = g_aclPendingCount.load(std::memory_order_relaxed); + return (v > 0) ? (uint16_t)v : 0; + } uint16_t AclMaxPackets() { return g_aclMaxNum; } + uint32_t AclTxInFlight() { + uint32_t tx = g_aclTxCount.load(std::memory_order_relaxed); + uint32_t done = g_aclTxDoneCount.load(std::memory_order_relaxed); + return (tx >= done) ? (tx - done) : 0; + } + + bool AclTxReady() { + // A send is safe only when BOTH resources have room: + // - a controller ACL buffer credit (NOCP-tracked), and + // - a TX DMA ring slot not still queued in the xHCI transfer ring. + // Leave one TX slot of headroom so a signaling reply fired from the + // event path (AVDTP/AVRCP response) can always go out. + uint16_t maxOut = g_aclMaxNum ? g_aclMaxNum : 4; + if (AclPendingCount() >= maxOut) return false; + if (AclTxInFlight() >= (uint32_t)(ACL_TX_SLOTS - 1)) return false; + return true; + } + + void AclResetCredits() { + // Lost-NOCP recovery: caller observed a prolonged credit stall with the + // USB side fully drained. Assume the completions were lost and start + // over; if the controller is genuinely still full it will NAK bulk OUT, + // which the TX-slot half of AclTxReady() still backpressures on. + g_aclPendingCount.store(0, std::memory_order_relaxed); + } + void DumpAclStats() { - KernelLogStream(INFO, "BT-HCI") << "ACL stats: tx=" << (uint64_t)g_aclTxCount - << " txDone=" << (uint64_t)g_aclTxDoneCount + KernelLogStream(INFO, "BT-HCI") << "ACL stats: tx=" + << (uint64_t)g_aclTxCount.load(std::memory_order_relaxed) + << " txDone=" << (uint64_t)g_aclTxDoneCount.load(std::memory_order_relaxed) << " rx=" << (uint64_t)g_aclRxCount << " rxDrop=" << (uint64_t)g_aclRxDropCount - << " pending=" << (uint64_t)g_aclPendingCount + << " pending=" << (uint64_t)AclPendingCount() << " bufNum=" << (uint64_t)g_aclMaxNum; } @@ -667,11 +708,9 @@ namespace Drivers::USB::Bluetooth::Hci { for (int i = 0; i < numHandles && (3 + i * 4) < evtParamLen; i++) { uint16_t completed = (uint16_t)params[3 + i * 4] | ((uint16_t)params[4 + i * 4] << 8); - if (g_aclPendingCount >= completed) { - g_aclPendingCount -= completed; - } else { - g_aclPendingCount = 0; - } + int32_t v = g_aclPendingCount.fetch_sub(completed, + std::memory_order_relaxed) - completed; + if (v < 0) g_aclPendingCount.store(0, std::memory_order_relaxed); } } break; @@ -1070,6 +1109,13 @@ namespace Drivers::USB::Bluetooth::Hci { // Top-level only: drains queued pairing replies with real, confirmed // control transfers. Must NOT be called from inside PollEvents. if (Xhci::InPollContext()) return; + // One consumer at a time: this runs from the Connect wait loop AND the + // idle-core service pump, potentially on different cores; two consumers + // on the lockless queue would race the tail and interleave commands. + static std::atomic s_active{false}; + bool expected = false; + if (!s_active.compare_exchange_strong(expected, true, + std::memory_order_acquire)) return; while (g_pendingTail != g_pendingHead) { PendingHciCmd c = g_pending[g_pendingTail]; g_pendingTail = (uint8_t)((g_pendingTail + 1) & 15); @@ -1082,6 +1128,7 @@ namespace Drivers::USB::Bluetooth::Hci { WaitCommandComplete(c.opcode, nullptr, 0, 1000); } } + s_active.store(false, std::memory_order_release); } bool WaitSecureSendResult(uint32_t timeoutMs, uint8_t* outResult, uint8_t* outStatus) { @@ -1316,6 +1363,18 @@ namespace Drivers::USB::Bluetooth::Hci { // ========================================================================= void DrainEvents() { + // One drainer at a time. This is called from every BT wait loop, the + // audio-write pump (syscall context, any core) and the idle-core + // service pump; the RX ring is single-consumer. Two cores inside the + // loop below would process the same slot twice (the duplicate + // Connection Request seen on HW) and advance the tail past unprocessed + // slots (the lost AVDTP Start response). Skipping is correct: the + // active drainer processes everything queued. + static std::atomic s_draining{false}; + bool expected = false; + if (!s_draining.compare_exchange_strong(expected, true, + std::memory_order_acquire)) return; + // Discard any unconsumed Command Complete/Status events that weren't // picked up by WaitCommandComplete/WaitCommandStatus. if (g_eventReady) { @@ -1349,6 +1408,8 @@ namespace Drivers::USB::Bluetooth::Hci { ProcessAcl(g_aclRxRing[slot], pl); g_aclRxTail = (uint8_t)((g_aclRxTail + 1) % ACL_RX_SLOTS); } + + s_draining.store(false, std::memory_order_release); } bool CreateConnection(const uint8_t* bdAddr) { diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.hpp b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp index 009e4eb..fa6c2e7 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Hci.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp @@ -310,6 +310,19 @@ namespace Drivers::USB::Bluetooth::Hci { uint16_t AclPendingCount(); uint16_t AclMaxPackets(); + // ACL packets handed to the xHCI whose bulk OUT completion has not been + // reaped yet (each one still owns a TX DMA ring slot). + uint32_t AclTxInFlight(); + + // True when SendAcl can be called without overrunning either the + // controller's ACL buffers (NOCP credits) or the TX DMA ring. The media + // writer polls events until this is true before sending each frame. + bool AclTxReady(); + + // Lost-NOCP recovery: zero the credit count after a prolonged stall with + // the USB side drained (completions presumed lost). + void AclResetCredits(); + // Disconnect a connection bool Disconnect(uint16_t handle, uint8_t reason); diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp index 414d12f..8630bc2 100644 --- a/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp @@ -7,6 +7,7 @@ #include "L2cap.hpp" #include "Hci.hpp" #include "A2dp.hpp" +#include "Avrcp.hpp" #include #include #include @@ -151,8 +152,8 @@ namespace Drivers::USB::Bluetooth::L2cap { KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM=" << base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid; - // Accept connections for AVDTP - if (psm == PSM_AVDTP || psm == PSM_SDP) { + // Accept connections for AVDTP, SDP, and AVRCP control + if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) { if (psm == PSM_AVDTP) g_incomingAvdtpReqs++; auto* ch = AllocChannel(psm); if (ch) { @@ -223,6 +224,19 @@ namespace Drivers::USB::Bluetooth::L2cap { break; } } + } else if (result == CONN_PENDING && dstCid != 0) { + // A PENDING response may already carry the remote's + // allocated CID. Record it (but do NOT configure -- + // the final SUCCESS on this channel does that): the + // diagnostics then show the peer engaged, and if the + // dial is later abandoned, FreeChannel knows to send + // a Disconnect for the half-open remote endpoint. + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) { + g_channels[i].RemoteCid = dstCid; + break; + } + } } } break; @@ -369,8 +383,27 @@ namespace Drivers::USB::Bluetooth::L2cap { break; } - default: + case SIG_DISCONN_RSP: + // Response to a Disconnect Request we sent (e.g. closing the + // SDP client channel after a query); nothing left to do. break; + + case SIG_ECHO_REQ: { + // Echo Request must be answered (echo the payload back) or + // a peer probing the link times out on us. + SendSignal(SIG_ECHO_RSP, sig->Identifier, sigPayload, sigPayloadLen); + break; + } + + default: { + // A signaling command we do not handle. Log it: a peer that + // sends something exotic while holding our dials at + // "authorization pending" is exactly what we need to see. + KernelLogStream(INFO, "BT-L2CAP") << "Unhandled signaling code=" + << base::hex << (uint64_t)sig->Code << base::dec + << " len=" << (uint64_t)sigPayloadLen; + break; + } } sOff += sizeof(SignalHeader) + sigPayloadLen; } @@ -381,7 +414,9 @@ namespace Drivers::USB::Bluetooth::L2cap { if (g_channels[i].Psm == PSM_AVDTP) { A2dp::ProcessAvdtp(payload, l2len); } else if (g_channels[i].Psm == PSM_SDP) { - A2dp::ProcessSdp(payload, l2len); + A2dp::ProcessSdp(cid, payload, l2len); + } else if (g_channels[i].Psm == PSM_AVCTP) { + Avrcp::ProcessAvctp(cid, payload, l2len); } break; } diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp index 465b72c..5fba285 100644 --- a/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp @@ -22,6 +22,7 @@ namespace Drivers::USB::Bluetooth::L2cap { // ========================================================================= constexpr uint16_t PSM_SDP = 0x0001; // Service Discovery Protocol + constexpr uint16_t PSM_AVCTP = 0x0017; // Audio/Video Control Transport (AVRCP) constexpr uint16_t PSM_AVDTP = 0x0019; // Audio/Video Distribution Transport // ========================================================================= @@ -51,6 +52,8 @@ namespace Drivers::USB::Bluetooth::L2cap { constexpr uint8_t SIG_CONFIG_RSP = 0x05; constexpr uint8_t SIG_DISCONN_REQ = 0x06; constexpr uint8_t SIG_DISCONN_RSP = 0x07; + constexpr uint8_t SIG_ECHO_REQ = 0x08; + constexpr uint8_t SIG_ECHO_RSP = 0x09; constexpr uint8_t SIG_INFO_REQ = 0x0A; constexpr uint8_t SIG_INFO_RSP = 0x0B; diff --git a/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp index 5680784..a4ae238 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp @@ -12,39 +12,51 @@ namespace Drivers::USB::Bluetooth::Sbc { // ========================================================================= - // Fixed-point constants (Q1.30 format for filter coefficients) + // Analysis filterbank tables // ========================================================================= - // Scale factor: 1.0 = (1 << 15) in Q15.16 - constexpr int32_t FP_ONE = (1 << 15); - - // SBC 8-subband analysis filter prototype coefficients (Q1.30) - // These are the 80 windowed prototype coefficients from the SBC spec - // Scaled to Q15.16 for our fixed-point arithmetic + // SBC 8-subband prototype window, the 80 coefficients from the A2DP spec + // (Annex B), natural order, signs built in, Q31 fixed point. Values match + // the spec floats exactly (e.g. [1] = 0x52173/2^31 = 1.56575398E-04). + // Verified on host: feeding subband-center sines concentrates energy in the + // right band with >63 dB rejection and overall gain 0.9997. static const int32_t g_proto8[80] = { - 0x0002, 0x0005, 0x000A, 0x0014, 0x0023, 0x0038, 0x0054, 0x0078, - 0x00A5, 0x00DB, 0x011B, 0x0164, 0x01B5, 0x020E, 0x026D, 0x02D0, - 0x0335, 0x039A, 0x03FC, 0x0458, 0x04AB, 0x04F1, 0x0527, 0x054A, - 0x0557, 0x054A, 0x0527, 0x04F1, 0x04AB, 0x0458, 0x03FC, 0x039A, - 0x0335, 0x02D0, 0x026D, 0x020E, 0x01B5, 0x0164, 0x011B, 0x00DB, - 0x00A5, 0x0078, 0x0054, 0x0038, 0x0023, 0x0014, 0x000A, 0x0005, - 0x0002, -0x0002, -0x0005, -0x000A, -0x0014, -0x0023, -0x0038, -0x0054, - -0x0078, -0x00A5, -0x00DB, -0x011B, -0x0164, -0x01B5, -0x020E, -0x026D, - -0x02D0, -0x0335, -0x039A, -0x03FC, -0x0458, -0x04AB, -0x04F1, -0x0527, - -0x054A, -0x0557, -0x054A, -0x0527, -0x04F1, -0x04AB, -0x0458, -0x03FC, + (int32_t)0x00000000, (int32_t)0x00052173, (int32_t)0x000B3F71, (int32_t)0x00122C7D, + (int32_t)0x001AFF89, (int32_t)0x00255A62, (int32_t)0x003060F4, (int32_t)0x003A72E7, + (int32_t)0x0041EC6A, (int32_t)0x0044EF48, (int32_t)0x00415B75, (int32_t)0x0034F8B6, + (int32_t)0x001D8FD2, (int32_t)0xFFFA2413, (int32_t)0xFFC9F10E, (int32_t)0xFF8D6793, + (int32_t)0x00B97348, (int32_t)0x01071B96, (int32_t)0x0156B3CA, (int32_t)0x01A1B38B, + (int32_t)0x01E0224C, (int32_t)0x0209291F, (int32_t)0x02138653, (int32_t)0x01F5F424, + (int32_t)0x01A7ECEF, (int32_t)0x01223EBA, (int32_t)0x005FD0FF, (int32_t)0xFF5EEB73, + (int32_t)0xFE20435D, (int32_t)0xFCA86E7E, (int32_t)0xFAFF95FC, (int32_t)0xF9312891, + (int32_t)0x08B4307A, (int32_t)0x0A9F3E9A, (int32_t)0x0C7D59B6, (int32_t)0x0E3BB16F, + (int32_t)0x0FC721F9, (int32_t)0x110ECEF0, (int32_t)0x120435FA, (int32_t)0x129C226F, + (int32_t)0x12CF6C75, (int32_t)0x129C226F, (int32_t)0x120435FA, (int32_t)0x110ECEF0, + (int32_t)0x0FC721F9, (int32_t)0x0E3BB16F, (int32_t)0x0C7D59B6, (int32_t)0x0A9F3E9A, + (int32_t)0xF74BCF86, (int32_t)0xF9312891, (int32_t)0xFAFF95FC, (int32_t)0xFCA86E7E, + (int32_t)0xFE20435D, (int32_t)0xFF5EEB73, (int32_t)0x005FD0FF, (int32_t)0x01223EBA, + (int32_t)0x01A7ECEF, (int32_t)0x01F5F424, (int32_t)0x02138653, (int32_t)0x0209291F, + (int32_t)0x01E0224C, (int32_t)0x01A1B38B, (int32_t)0x0156B3CA, (int32_t)0x01071B96, + (int32_t)0xFF468CB8, (int32_t)0xFF8D6793, (int32_t)0xFFC9F10E, (int32_t)0xFFFA2413, + (int32_t)0x001D8FD2, (int32_t)0x0034F8B6, (int32_t)0x00415B75, (int32_t)0x0044EF48, + (int32_t)0x0041EC6A, (int32_t)0x003A72E7, (int32_t)0x003060F4, (int32_t)0x00255A62, + (int32_t)0x001AFF89, (int32_t)0x00122C7D, (int32_t)0x000B3F71, (int32_t)0x00052173, }; - // Cosine matrix for 8-subband DCT-II (Q15.16) - // cos_matrix[k][i] = cos((k + 0.5) * (2*i + 1) * PI / 16) * FP_ONE - static const int32_t g_cosMatrix8[8][16] = { - { 32138, 31650, 30679, 29246, 27381, 25126, 22529, 19644, 16531, 13254, 9882, 6484, 3134, -199, -3509, -6758}, - { 30679, 25126, 16531, 6484, -3509,-13254,-22529,-29246,-32138,-31650,-27381,-19644, -9882, 199, 9882, 19644}, - { 27381, 13254, -3509,-19644,-30679,-32138,-22529, -6484, 9882, 25126, 32138, 29246, 16531, -199,-16531,-29246}, - { 22529, -199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199}, - { 16531,-13254,-30679, 6484, 32138, -199,-32138, -6484, 30679, 13254,-16531,-25126, 3509, 29246, 9882,-27381}, - { 9882,-25126,-16531, 29246, 3509,-32138, 9882, 25126,-16531,-29246, 3509, 32138, -9882,-25126, 16531, 29246}, - { 3134,-31650, 27381, -6484,-22529, 32138,-13254, -9882, 30679,-29246, 6484, 19644,-32138, 16531, 3509,-25126}, - { -3509, 32138,-22529, -6484, 30679,-27381, 3509, 25126,-32138, 16531, 9882,-30679, 22529, -199,-25126, 32138}, + // Analysis matrixing for 8 subbands (A2DP spec Annex B), Q14: + // M[k][i] = cos((i - 4) * (2k + 1) * pi / 16) * 2^14 + // (The (i-4) phase is the one whose aliasing cancels in the reference + // decoder's synthesis; (i+4) passes an energy-concentration test equally + // but reconstructs with frequency-dependent distortion.) + static const int16_t g_cosMatrix8[8][16] = { + { 11585, 13623, 15137, 16069, 16384, 16069, 15137, 13623, 11585, 9102, 6270, 3196, 0, -3196, -6270, -9102 }, + { -11585, -3196, 6270, 13623, 16384, 13623, 6270, -3196, -11585, -16069, -15137, -9102, 0, 9102, 15137, 16069 }, + { -11585, -16069, -6270, 9102, 16384, 9102, -6270, -16069, -11585, 3196, 15137, 13623, 0, -13623, -15137, -3196 }, + { 11585, -9102, -15137, 3196, 16384, 3196, -15137, -9102, 11585, 13623, -6270, -16069, 0, 16069, 6270, -13623 }, + { 11585, 9102, -15137, -3196, 16384, -3196, -15137, 9102, 11585, -13623, -6270, 16069, 0, -16069, 6270, 13623 }, + { -11585, 16069, -6270, -9102, 16384, -9102, -6270, 16069, -11585, -3196, 15137, -13623, 0, 13623, -15137, 3196 }, + { -11585, 3196, 6270, -13623, 16384, -13623, 6270, 3196, -11585, 16069, -15137, 9102, 0, -9102, 15137, -16069 }, + { 11585, -13623, 15137, -16069, 16384, -16069, 15137, -13623, 11585, -9102, 6270, -3196, 0, 3196, -6270, 9102 }, }; // ========================================================================= @@ -138,37 +150,50 @@ namespace Drivers::USB::Bluetooth::Sbc { // Analysis filter bank (8 subbands) // ========================================================================= + // Direct implementation of the spec pseudocode (8 subbands): + // shift X by 8; insert the 8 new samples NEWEST-FIRST (X[0] = newest) + // Y[i] = sum_{j=0..4} X[i+16j] * C[i+16j] (window, 80 taps) + // S[k] = sum_{i=0..15} M[k][i] * Y[i] (cosine matrixing) + // Fixed point: X holds 16-bit samples, C is Q31, M is Q14. The window + // accumulator keeps Y at sample*2^15 (>>16 off Q31), the matrix adds 14 + // bits, so >>30 would land S at HALF input-sample units -- the 0.5x + // analysis gain the SBC synthesis filterbank expects (its synthesis + // doubles; verified against the BlueZ reference decoder: 1.0x decodes + // 6 dB hot). We shift FRAC_BITS less to carry fractional precision + // through quantization: at bitpool 53 the quantizer step is often finer + // than one input-sample unit, so integer subband samples would be the + // noise floor (costs ~10 dB on sweeps). Scale factors and the quantizer + // account for FRAC_BITS; full-scale input still peaks at sf = 14. + // Extra fractional bits carried in sb_samples below the half-input-unit + // point (subband sample value = signal * 2^FRAC_BITS). + constexpr int FRAC_BITS = 12; + static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch, int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) { + int32_t* X = enc->X[ch]; for (int blk = 0; blk < enc->Blocks; blk++) { - // Shift in new samples - int pos = enc->XPos[ch]; - for (int i = enc->Subbands - 1; i >= 0; i--) { - pos = (pos + 1) % (enc->Subbands * 10); - enc->X[ch][pos] = (int32_t)pcm[blk * enc->Subbands * enc->Channels + i * enc->Channels + ch]; + for (int i = 79; i >= 8; i--) X[i] = X[i - 8]; + const int16_t* in = pcm + blk * 8 * enc->Channels + ch; + for (int i = 0; i < 8; i++) { + X[i] = (int32_t)in[(7 - i) * enc->Channels]; } - enc->XPos[ch] = pos; - // Windowing and partial calculation - int32_t Z[2 * SBC_SUBBANDS]; - for (int i = 0; i < 2 * enc->Subbands; i++) { - Z[i] = 0; + int32_t Y[16]; + for (int i = 0; i < 16; i++) { + int64_t acc = 0; for (int j = 0; j < 5; j++) { - int idx = (pos + i + j * 2 * enc->Subbands) % (enc->Subbands * 10); - int protoIdx = i + j * 2 * enc->Subbands; - if (protoIdx < 80) { - Z[i] += (int32_t)(((int64_t)enc->X[ch][idx] * g_proto8[protoIdx]) >> 15); - } + acc += (int64_t)X[i + 16 * j] * g_proto8[i + 16 * j]; } + Y[i] = (int32_t)((acc + (1 << 15)) >> 16); } - // Matrixing (DCT) - for (int k = 0; k < enc->Subbands; k++) { - int32_t sum = 0; - for (int i = 0; i < 2 * enc->Subbands; i++) { - sum += (int32_t)(((int64_t)Z[i] * g_cosMatrix8[k][i]) >> 15); + for (int k = 0; k < 8; k++) { + int64_t s = 0; + for (int i = 0; i < 16; i++) { + s += (int64_t)Y[i] * g_cosMatrix8[k][i]; } - sb_samples[blk][k] = sum; + sb_samples[blk][k] = + (int32_t)((s + ((int64_t)1 << (29 - FRAC_BITS))) >> (30 - FRAC_BITS)); } } } @@ -191,7 +216,7 @@ namespace Drivers::USB::Bluetooth::Sbc { if (val < 0) val = -val; if (val > maxVal) maxVal = val; } - int32_t sf = 0, tmp = maxVal; + int32_t sf = 0, tmp = maxVal >> FRAC_BITS; while (tmp > 0) { sf++; tmp >>= 1; } scale_factors[sb] = sf; } @@ -344,42 +369,50 @@ namespace Drivers::USB::Bluetooth::Sbc { AnalysisFilter(enc, pcm, ch, sb_samples[ch]); } - // 2. Joint-stereo decision + mid/side transform, applied BEFORE scale - // factors and bit allocation: the decoder re-derives the bit widths - // from the transmitted scale factors, which must reflect the FINAL - // (post-transform) samples, or the sample bitstream desyncs. + // 2. Scale factors from the subband samples. + for (int ch = 0; ch < enc->Channels; ch++) { + ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]); + } + + // 3. Joint-stereo decision + mid/side transform. A band is joined + // when it saves scale-factor bits: sfL + sfR > sfM + sfS (the + // reference encoder's criterion). Updates BOTH samples and scale + // factors before bit allocation: the decoder re-derives the bit + // widths from the transmitted scale factors, which must reflect + // the FINAL (post-transform) samples, or the bitstream desyncs. + // The last subband never joins (carries the side channel's escape). uint8_t joint = 0; if (enc->ChannelMode == MODE_JOINT_STEREO) { for (int sb = 0; sb < enc->Subbands - 1; sb++) { - int32_t maxMid = 0, maxSide = 0, maxOrig = 0; + int32_t mid[SBC_BLOCKS], side[SBC_BLOCKS]; + int32_t maxMid = 0, maxSide = 0; for (int blk = 0; blk < enc->Blocks; blk++) { int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb]; - int32_t mid = (l + r) / 2, side = (l - r) / 2; - if (mid < 0) mid = -mid; - if (side < 0) side = -side; - int32_t al = l < 0 ? -l : l, ar = r < 0 ? -r : r; - if (mid > maxMid) maxMid = mid; - if (side > maxSide) maxSide = side; - if (al > maxOrig) maxOrig = al; - if (ar > maxOrig) maxOrig = ar; + mid[blk] = (l >> 1) + (r >> 1); + side[blk] = (l >> 1) - (r >> 1); + int32_t am = mid[blk] < 0 ? -mid[blk] : mid[blk]; + int32_t as = side[blk] < 0 ? -side[blk] : side[blk]; + if (am > maxMid) maxMid = am; + if (as > maxSide) maxSide = as; } - if (maxMid + maxSide < maxOrig) { - joint |= (1 << (enc->Subbands - 1 - sb)); + int32_t sfm = 0, tmp = maxMid >> FRAC_BITS; + while (tmp > 0) { sfm++; tmp >>= 1; } + int32_t sfs = 0; + tmp = maxSide >> FRAC_BITS; + while (tmp > 0) { sfs++; tmp >>= 1; } + + if (scale_factors[0][sb] + scale_factors[1][sb] > sfm + sfs) { + joint |= (uint8_t)(1 << (enc->Subbands - 1 - sb)); + scale_factors[0][sb] = sfm; + scale_factors[1][sb] = sfs; for (int blk = 0; blk < enc->Blocks; blk++) { - int32_t l = sb_samples[0][blk][sb]; - int32_t r = sb_samples[1][blk][sb]; - sb_samples[0][blk][sb] = (l + r) / 2; - sb_samples[1][blk][sb] = (l - r) / 2; + sb_samples[0][blk][sb] = mid[blk]; + sb_samples[1][blk][sb] = side[blk]; } } } } - // 3. Scale factors from the final subband samples. - for (int ch = 0; ch < enc->Channels; ch++) { - ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]); - } - // 4. Bit allocation (canonical; decoder re-derives the same widths). BitAllocation(enc, scale_factors, bits); @@ -434,13 +467,13 @@ namespace Drivers::USB::Bluetooth::Sbc { int32_t sf = scale_factors[ch][sb]; int32_t sample = sb_samples[ch][blk][sb]; uint32_t levels = (1u << bits[ch][sb]) - 1; - int32_t quantized; - if (sf > 0) { - int32_t maxRange = (1 << sf); - quantized = (int32_t)(((int64_t)(sample + maxRange) * levels) / (2 * maxRange)); - } else { - quantized = levels / 2; - } + // q = levels * (sample + range) / (2 * range), truncating, + // with range = 2^(sf + FRAC_BITS) -- same mapping as the + // reference encoder. The fractional sample bits feed + // straight into the quantizer's precision. + int32_t range = 1 << (sf + FRAC_BITS); + int32_t quantized = (int32_t) + (((int64_t)(sample + range) * levels) >> (sf + FRAC_BITS + 1)); if (quantized < 0) quantized = 0; if (quantized > (int32_t)levels) quantized = (int32_t)levels; WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]); diff --git a/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp b/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp index 9a1c611..84ae6a1 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp @@ -57,9 +57,9 @@ namespace Drivers::USB::Bluetooth::Sbc { uint8_t Bitpool; uint8_t Channels; - // Analysis filter state (per-channel windowed buffer) + // Analysis filter state (per-channel windowed buffer; X[0] = newest + // sample, shifted down by Subbands each block) int32_t X[SBC_CHANNELS][SBC_SUBBANDS * 10]; - int XPos[SBC_CHANNELS]; // Computed frame size in bytes uint32_t FrameSize; diff --git a/kernel/src/Drivers/USB/Xhci.cpp b/kernel/src/Drivers/USB/Xhci.cpp index 2850162..bcae04f 100644 --- a/kernel/src/Drivers/USB/Xhci.cpp +++ b/kernel/src/Drivers/USB/Xhci.cpp @@ -5,6 +5,7 @@ */ #include "Xhci.hpp" +#include #include "UsbDevice.hpp" #include "HidKeyboard.hpp" #include "HidMouse.hpp" @@ -112,7 +113,17 @@ namespace Drivers::USB::Xhci { // Bluetooth event handler) see this and must NOT wait for completion -- // PollEvents is non-reentrant, so the wait would never observe the // completion. Used by InPollContext() / ControlTransfer(). - static volatile bool g_pollActive = false; + // + // MUST be a real atomic claimed with compare-exchange, not a plain + // volatile bool: PollEvents runs from the MSI handler (BSP) AND from + // syscall-context pumps (A2DP WriteAudio/StartSource on whatever core the + // app runs on) AND from idle cores (ProcessDeferredWork). A check-then-set + // on a volatile is a cross-core TOCTOU: two cores both pass the check, + // both drain the shared event ring, g_evtRingDequeue/g_evtRingCCS desync, + // and completions get double-delivered (duplicate ACL packets) or lost + // (missed AVDTP responses) -- both observed on HW once audio streaming + // started pumping from syscall context. + static std::atomic g_pollActive{false}; // Interrupt transfer data buffers (per slot) static uint8_t* g_interruptDataBuf[MAX_SLOTS + 1] = {}; @@ -377,7 +388,7 @@ namespace Drivers::USB::Xhci { // sent from a Bluetooth event handler). Such callers must fire-and-forget, // not wait, since a nested PollEvents is a no-op. bool InPollContext() { - return g_pollActive; + return g_pollActive.load(std::memory_order_relaxed); } // ------------------------------------------------------------------------- @@ -396,8 +407,11 @@ namespace Drivers::USB::Xhci { // the active poll loop drains these events itself. g_pollActive is also // read by InPollContext() so command submitters (ControlTransfer) can // tell they are nested and must fire-and-forget instead of waiting. - if (g_pollActive) return; - g_pollActive = true; + bool expected = false; + if (!g_pollActive.compare_exchange_strong(expected, true, + std::memory_order_acquire)) { + return; // another context is draining; it will reap our events + } // Bound the work per call so a flooding/wedged device can never spin // here forever; the outer wall-clock timeouts then fire instead of @@ -566,7 +580,7 @@ namespace Drivers::USB::Xhci { } } - g_pollActive = false; + g_pollActive.store(false, std::memory_order_release); } // ------------------------------------------------------------------------- @@ -726,7 +740,7 @@ namespace Drivers::USB::Xhci { // would never be observed and the timeout+recovery path would corrupt // the EP0 ring. The transfer is submitted (doorbell rung); let the // active PollEvents reap its completion. Fire-and-forget. - if (g_pollActive) { + if (g_pollActive.load(std::memory_order_relaxed)) { return CC_SUCCESS; } diff --git a/kernel/src/Timekeeping/ApicTimer.cpp b/kernel/src/Timekeeping/ApicTimer.cpp index 70562df..ce31515 100644 --- a/kernel/src/Timekeeping/ApicTimer.cpp +++ b/kernel/src/Timekeeping/ApicTimer.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include using namespace Kt; @@ -211,6 +212,15 @@ namespace Timekeeping { Drivers::USB::Xhci::ProcessDeferredWork(); } + // Service Bluetooth inbound traffic (the headset's SDP/AVRCP queries, + // AVDTP commands, ACL flow-control credits) whenever a core idles. + // Without this, BT events are only processed while some syscall happens + // to be pumping a wait loop -- a peer that queries us between audio + // writes got silence (observed: Bose re-dialing SDP during playback, + // queries never answered). Self-serializing and a cheap no-op when + // the adapter is down. + Drivers::USB::Bluetooth::ServiceEvents(); + if (cpu == nullptr || cpu->cpuIndex != 0 || !g_schedEnabled || g_ticksPerMs == 0) { // Non-BSP or pre-scheduler fallback: plain HLT keeps the LAPIC // timer running in C1 on hardware where MWAIT promotes to deeper