feat: A2DP bluetooth audio working
This commit is contained in:
@@ -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 <marcel@holtmann.org>
|
|
||||||
*
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
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);
|
|
||||||
@@ -10,6 +10,7 @@
|
|||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <Drivers/Audio/IntelHda.hpp>
|
#include <Drivers/Audio/IntelHda.hpp>
|
||||||
#include <Drivers/Audio/Mixer.hpp>
|
#include <Drivers/Audio/Mixer.hpp>
|
||||||
|
#include <Terminal/Terminal.hpp>
|
||||||
#include <Drivers/USB/Bluetooth/Bluetooth.hpp>
|
#include <Drivers/USB/Bluetooth/Bluetooth.hpp>
|
||||||
#include <Drivers/USB/Bluetooth/A2dp.hpp>
|
#include <Drivers/USB/Bluetooth/A2dp.hpp>
|
||||||
#include <Sched/Scheduler.hpp>
|
#include <Sched/Scheduler.hpp>
|
||||||
@@ -35,9 +36,16 @@ namespace Montauk {
|
|||||||
state == Drivers::USB::Bluetooth::A2dp::State::Streaming ||
|
state == Drivers::USB::Bluetooth::A2dp::State::Streaming ||
|
||||||
state == Drivers::USB::Bluetooth::A2dp::State::Configured) {
|
state == Drivers::USB::Bluetooth::A2dp::State::Configured) {
|
||||||
Drivers::USB::Bluetooth::A2dp::ConfigureStream(sampleRate, channels, bitsPerSample);
|
Drivers::USB::Bluetooth::A2dp::ConfigureStream(sampleRate, channels, bitsPerSample);
|
||||||
Drivers::USB::Bluetooth::A2dp::StartStream();
|
if (Drivers::USB::Bluetooth::A2dp::StartStream()) {
|
||||||
return AUDIO_HANDLE_BT;
|
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";
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// HDA-backed mixer is the default output. The mixer keeps the HDA
|
// HDA-backed mixer is the default output. The mixer keeps the HDA
|
||||||
@@ -56,7 +64,7 @@ namespace Montauk {
|
|||||||
|
|
||||||
static int64_t Sys_AudioClose(int handle) {
|
static int64_t Sys_AudioClose(int handle) {
|
||||||
if (handle == AUDIO_HANDLE_BT) {
|
if (handle == AUDIO_HANDLE_BT) {
|
||||||
Drivers::USB::Bluetooth::A2dp::StopStream();
|
Drivers::USB::Bluetooth::A2dp::StopStream(true); // drop queued tail
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
Drivers::Audio::Mixer::Close(handle);
|
Drivers::Audio::Mixer::Close(handle);
|
||||||
@@ -79,7 +87,8 @@ namespace Montauk {
|
|||||||
case AUDIO_CTL_GET_VOLUME:
|
case AUDIO_CTL_GET_VOLUME:
|
||||||
return Drivers::USB::Bluetooth::A2dp::GetVolume();
|
return Drivers::USB::Bluetooth::A2dp::GetVolume();
|
||||||
case AUDIO_CTL_PAUSE:
|
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();
|
else Drivers::USB::Bluetooth::A2dp::StartStream();
|
||||||
return 0;
|
return 0;
|
||||||
case AUDIO_CTL_GET_OUTPUT:
|
case AUDIO_CTL_GET_OUTPUT:
|
||||||
|
|||||||
@@ -12,4 +12,4 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define MONTAUK_BUILD_NUMBER 3
|
#define MONTAUK_BUILD_NUMBER 12
|
||||||
|
|||||||
@@ -13,6 +13,7 @@
|
|||||||
#include <CppLib/Stream.hpp>
|
#include <CppLib/Stream.hpp>
|
||||||
#include <Libraries/Memory.hpp>
|
#include <Libraries/Memory.hpp>
|
||||||
#include <Timekeeping/ApicTimer.hpp>
|
#include <Timekeeping/ApicTimer.hpp>
|
||||||
|
#include <atomic>
|
||||||
|
|
||||||
using namespace Kt;
|
using namespace Kt;
|
||||||
|
|
||||||
@@ -33,6 +34,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
constexpr uint8_t AVDTP_CLOSE = 0x08;
|
constexpr uint8_t AVDTP_CLOSE = 0x08;
|
||||||
constexpr uint8_t AVDTP_SUSPEND = 0x09;
|
constexpr uint8_t AVDTP_SUSPEND = 0x09;
|
||||||
constexpr uint8_t AVDTP_ABORT = 0x0A;
|
constexpr uint8_t AVDTP_ABORT = 0x0A;
|
||||||
|
constexpr uint8_t AVDTP_GET_ALL_CAPABILITIES = 0x0C; // AVDTP 1.3
|
||||||
|
|
||||||
// AVDTP message types
|
// AVDTP message types
|
||||||
constexpr uint8_t MSG_COMMAND = 0x00;
|
constexpr uint8_t MSG_COMMAND = 0x00;
|
||||||
@@ -109,6 +111,29 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
static uint16_t g_seqNum = 0;
|
static uint16_t g_seqNum = 0;
|
||||||
static uint32_t g_timestamp = 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<uint32_t> g_ringHead{0}; // producer: WriteAudio
|
||||||
|
static std::atomic<uint32_t> g_ringTail{0}; // consumer: PumpMedia
|
||||||
|
static std::atomic<bool> 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
|
// Volume
|
||||||
static int g_volume = 80;
|
static int g_volume = 80;
|
||||||
|
|
||||||
@@ -409,22 +434,40 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
|
|
||||||
static bool AvdtpStart() {
|
static bool AvdtpStart() {
|
||||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||||
|
|
||||||
|
for (int attempt = 0; attempt < 2; attempt++) {
|
||||||
SendAvdtpCommand(AVDTP_START, payload, 1);
|
SendAvdtpCommand(AVDTP_START, payload, 1);
|
||||||
|
|
||||||
if (!WaitAvdtpResponse()) {
|
if (!WaitAvdtpResponse()) {
|
||||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout";
|
// A lost response is recoverable: re-issue once. AVDTP START
|
||||||
return false;
|
// 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;
|
||||||
}
|
}
|
||||||
if (!AvdtpAccepted()) {
|
|
||||||
uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0;
|
|
||||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start rejected (err="
|
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start rejected (err="
|
||||||
<< base::hex << (uint64_t)err << base::dec << ")";
|
<< base::hex << (uint64_t)err << base::dec << ")";
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
return false;
|
||||||
g_state = State::Streaming;
|
|
||||||
KernelLogStream(OK, "BT-A2DP") << "Streaming started";
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
@@ -459,10 +502,435 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
// the Open state; the first audio write (StartStream) issues AVDTP_START.
|
// the Open state; the first audio write (StartStream) issues AVDTP_START.
|
||||||
// Without this the headset has no media stream and terminates the link
|
// Without this the headset has no media stream and terminates the link
|
||||||
// (HCI disconnect reason 0x13), which is the connect/disconnect flapping.
|
// (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) {
|
// SDP server
|
||||||
(void)data;
|
// =========================================================================
|
||||||
if (len > 0) g_sdpRspReady = true;
|
// 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
|
// 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));
|
L2cap::SendData(cid, pdu, sizeof(pdu));
|
||||||
|
|
||||||
|
bool answered = false;
|
||||||
uint64_t start = Timekeeping::GetMilliseconds();
|
uint64_t start = Timekeeping::GetMilliseconds();
|
||||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||||
Xhci::PollEvents();
|
Xhci::PollEvents();
|
||||||
Hci::DrainEvents();
|
Hci::DrainEvents();
|
||||||
if (g_sdpRspReady) {
|
if (g_sdpRspReady) { answered = true; break; }
|
||||||
KernelLogStream(OK, "BT-A2DP") << "SDP query answered";
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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)";
|
KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)";
|
||||||
|
}
|
||||||
return true; // channel configured + query sent; proceed to AVDTP
|
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
|
// and abandoned the very connection the sink was authorizing, while a
|
||||||
// single held dial (build 44) already proved holding alone is harmless.
|
// single held dial (build 44) already proved holding alone is harmless.
|
||||||
// Also accept an inbound transport channel (some sinks open it).
|
// Also accept an inbound transport channel (some sinks open it).
|
||||||
// NOTE: the real gate on Bose is Content Protection -- see
|
// NOTE: Bose service-gates this channel TWICE: SetConfiguration must
|
||||||
// AvdtpSetConfiguration's SCMS-T handling; without it the sink pends
|
// include Content Protection (SCMS-T, see AvdtpSetConfiguration), AND
|
||||||
// this channel forever (connRsp=1, remoteCid=0).
|
// 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;
|
g_mediaCid = 0;
|
||||||
constexpr uint32_t kMediaWaitMs = 8000;
|
constexpr uint32_t kMediaWaitMs = 8000;
|
||||||
|
|
||||||
@@ -671,6 +1153,25 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
|
|
||||||
KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid="
|
KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid="
|
||||||
<< base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open";
|
<< 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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -711,8 +1212,11 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case AVDTP_GET_CAPABILITIES: {
|
case AVDTP_GET_CAPABILITIES:
|
||||||
// Respond with our SBC 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] = {};
|
uint8_t rsp[10] = {};
|
||||||
rsp[0] = CAT_MEDIA_TRANSPORT;
|
rsp[0] = CAT_MEDIA_TRANSPORT;
|
||||||
rsp[1] = 0;
|
rsp[1] = 0;
|
||||||
@@ -724,7 +1228,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0)
|
rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0)
|
||||||
rsp[8] = 2; // Min bitpool
|
rsp[8] = 2; // Min bitpool
|
||||||
rsp[9] = 53; // Max bitpool
|
rsp[9] = 53; // Max bitpool
|
||||||
SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10);
|
SendAvdtpResponse(txLabel, signalId, rsp, 10);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -751,34 +1255,52 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
|
|
||||||
case AVDTP_START: {
|
case AVDTP_START: {
|
||||||
g_state = State::Streaming;
|
g_state = State::Streaming;
|
||||||
|
ResetMediaClock();
|
||||||
SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0);
|
SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0);
|
||||||
KernelLogStream(OK, "BT-A2DP") << "Remote started streaming";
|
KernelLogStream(OK, "BT-A2DP") << "Remote started streaming";
|
||||||
break;
|
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: {
|
case AVDTP_CLOSE: {
|
||||||
g_state = State::Idle;
|
g_state = State::Idle;
|
||||||
SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0);
|
SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0);
|
||||||
|
KernelLogStream(WARNING, "BT-A2DP") << "Remote CLOSED stream";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case AVDTP_SUSPEND: {
|
case AVDTP_SUSPEND: {
|
||||||
g_state = State::Open;
|
g_state = State::Open;
|
||||||
SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0);
|
SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0);
|
||||||
|
KernelLogStream(WARNING, "BT-A2DP") << "Remote SUSPENDED stream";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case AVDTP_ABORT: {
|
case AVDTP_ABORT: {
|
||||||
g_state = State::Idle;
|
g_state = State::Idle;
|
||||||
SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0);
|
SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0);
|
||||||
|
KernelLogStream(WARNING, "BT-A2DP") << "Remote ABORTED stream";
|
||||||
break;
|
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;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
// ConfigureStream
|
// ConfigureStream
|
||||||
@@ -795,6 +1317,11 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
g_sbcInitialized = true;
|
g_sbcInitialized = true;
|
||||||
g_seqNum = 0;
|
g_seqNum = 0;
|
||||||
g_timestamp = 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: "
|
KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: "
|
||||||
<< (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit "
|
<< (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit "
|
||||||
@@ -812,63 +1339,114 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
if (g_state == State::Configured) {
|
if (g_state == State::Configured) {
|
||||||
if (!AvdtpOpen()) return false;
|
if (!AvdtpOpen()) return false;
|
||||||
}
|
}
|
||||||
return AvdtpStart();
|
if (!AvdtpStart()) return false;
|
||||||
|
ResetMediaClock();
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
return (g_state == State::Streaming);
|
return (g_state == State::Streaming);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StopStream() {
|
bool StopStream(bool flushQueued) {
|
||||||
if (g_state == State::Streaming) {
|
if (g_state == State::Streaming) {
|
||||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||||
SendAvdtpCommand(AVDTP_SUSPEND, payload, 1);
|
SendAvdtpCommand(AVDTP_SUSPEND, payload, 1);
|
||||||
WaitAvdtpResponse(1000);
|
WaitAvdtpResponse(1000);
|
||||||
g_state = State::Open;
|
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;
|
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) {
|
void PumpMedia() {
|
||||||
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) {
|
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) return;
|
||||||
return -1;
|
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 samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder);
|
||||||
uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; // 16-bit samples
|
uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2;
|
||||||
uint32_t sbcFrameSize = Sbc::GetFrameSize(&g_sbcEncoder);
|
int16_t framePcm[512];
|
||||||
|
|
||||||
// Apply volume scaling to PCM data
|
while (bytesPerFrame <= sizeof(framePcm) && g_pcmRate != 0) {
|
||||||
// We work on a local copy for volume adjustment
|
if (g_state != State::Streaming) break; // torn down mid-pump
|
||||||
int16_t scaledPcm[512]; // Max ~128 samples * 2 channels = 256 samples
|
|
||||||
if (bytesPerFrame > sizeof(scaledPcm)) return -1;
|
|
||||||
|
|
||||||
uint32_t consumed = 0;
|
uint32_t fill = g_ringHead.load(std::memory_order_acquire)
|
||||||
uint16_t maxOut = Hci::AclMaxPackets();
|
- g_ringTail.load(std::memory_order_relaxed);
|
||||||
if (maxOut == 0) maxOut = 4; // controller ACL buffer credits
|
uint64_t now = Timekeeping::GetMilliseconds();
|
||||||
|
uint64_t audioMs = g_sentSamples * 1000 / g_pcmRate;
|
||||||
|
uint64_t elapsed = now - g_clockBase;
|
||||||
|
|
||||||
while (consumed + bytesPerFrame <= pcmLen) {
|
if (fill < bytesPerFrame) {
|
||||||
// Copy and scale by volume
|
// Ring dry. If the sink's lead is exhausted too, this is an
|
||||||
const int16_t* src = (const int16_t*)(pcmData + consumed);
|
// audible dropout caused by the app not feeding in time.
|
||||||
uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels;
|
if (!g_inUnderrun && g_sentSamples != 0 && elapsed > audioMs) {
|
||||||
for (uint32_t i = 0; i < numSamples; i++) {
|
g_inUnderrun = true;
|
||||||
scaledPcm[i] = (int16_t)(((int32_t)src[i] * g_volume) / 100);
|
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
|
// Pull one frame from the ring (may wrap) and apply volume.
|
||||||
uint8_t mediaPkt[256] = {};
|
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)
|
uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels;
|
||||||
// Byte 0: V=2, P=0, X=0, CC=0 -> 0x80
|
for (uint32_t i = 0; i < numSamples; i++) {
|
||||||
// Byte 1: M=0, PT=96 -> 0x60
|
framePcm[i] = (int16_t)(((int32_t)framePcm[i] * g_volume) / 100);
|
||||||
// Bytes 2-3: Sequence number
|
}
|
||||||
// Bytes 4-7: Timestamp
|
|
||||||
// Bytes 8-11: SSRC
|
// Build media packet: RTP-like header (12 bytes) + optional SCMS-T
|
||||||
// Byte 12: SBC payload header (number of SBC frames)
|
// content-protection header (1 byte) + SBC payload header (1 byte)
|
||||||
mediaPkt[0] = 0x80;
|
// + SBC frame
|
||||||
mediaPkt[1] = 0x60;
|
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[2] = (uint8_t)(g_seqNum >> 8);
|
||||||
mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF);
|
mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF);
|
||||||
mediaPkt[4] = (uint8_t)(g_timestamp >> 24);
|
mediaPkt[4] = (uint8_t)(g_timestamp >> 24);
|
||||||
@@ -876,37 +1454,86 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
mediaPkt[6] = (uint8_t)(g_timestamp >> 8);
|
mediaPkt[6] = (uint8_t)(g_timestamp >> 8);
|
||||||
mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF);
|
mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF);
|
||||||
mediaPkt[8] = 0; mediaPkt[9] = 0; mediaPkt[10] = 0; mediaPkt[11] = 0x01; // SSRC
|
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
|
// When SCMS-T was configured (SetConfiguration cat 0x04), every
|
||||||
uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, scaledPcm, &mediaPkt[13]);
|
// 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;
|
uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, framePcm, &mediaPkt[hdr]);
|
||||||
|
L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)(hdr + 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();
|
|
||||||
|
|
||||||
g_seqNum++;
|
g_seqNum++;
|
||||||
g_timestamp += samplesPerFrame;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
|
|||||||
@@ -38,8 +38,10 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
// Process an AVDTP signaling packet
|
// Process an AVDTP signaling packet
|
||||||
void ProcessAvdtp(const uint8_t* data, uint16_t len);
|
void ProcessAvdtp(const uint8_t* data, uint16_t len);
|
||||||
|
|
||||||
// Process an SDP response packet (on the SDP L2CAP channel)
|
// Process an SDP packet (on any SDP L2CAP channel). Dispatches by PDU:
|
||||||
void ProcessSdp(const uint8_t* data, uint16_t len);
|
// 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
|
// Configure a stream for the given PCM parameters
|
||||||
bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample);
|
bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample);
|
||||||
@@ -47,13 +49,21 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
|||||||
// Start streaming
|
// Start streaming
|
||||||
bool StartStream();
|
bool StartStream();
|
||||||
|
|
||||||
// Stop streaming
|
// Stop streaming. flushQueued drops any PCM still queued in the media
|
||||||
bool StopStream();
|
// 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
|
// Write PCM audio data to the Bluetooth audio stream. Copies into the
|
||||||
// Returns number of bytes consumed
|
// 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);
|
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
|
// Get current state
|
||||||
State GetState();
|
State GetState();
|
||||||
|
|
||||||
|
|||||||
@@ -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 <Terminal/Terminal.hpp>
|
||||||
|
#include <CppLib/Stream.hpp>
|
||||||
|
#include <Libraries/Memory.hpp>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,19 @@
|
|||||||
|
/*
|
||||||
|
* Avrcp.hpp
|
||||||
|
* Bluetooth AVRCP (Audio/Video Remote Control Profile) -- minimal Target role
|
||||||
|
* Copyright (c) 2026 Daniel Hammer
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
}
|
||||||
@@ -218,15 +218,32 @@ namespace Drivers::USB::Bluetooth {
|
|||||||
// Set local name
|
// Set local name
|
||||||
Hci::WriteLocalName("MontaukOS");
|
Hci::WriteLocalName("MontaukOS");
|
||||||
|
|
||||||
// Set class of device: Audio (Major Service: Audio, Major Class: Audio/Video)
|
// Class of Device. The A2DP spec MANDATES the Capturing service bit
|
||||||
// CoD: 0x240404 = Rendering | Audio | Wearable Headset (common for audio devices)
|
// (0x080000) for a source; Audio (0x200000) is customary. Major/minor
|
||||||
// For a computer acting as audio source:
|
// class: Computer/Laptop (0x010C) -- what we actually are. The old
|
||||||
// 0x200408 = Audio service | Audio/Video class | Portable Audio
|
// value 0x200408 (Audio/Video major class, minor "hands-free device",
|
||||||
Hci::WriteClassOfDevice(0x200408);
|
// 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
|
// Enable Simple Secure Pairing
|
||||||
Hci::WriteSSPMode(1);
|
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)
|
// Set event mask to receive relevant events. Octet 6 (events 0x31-0x38)
|
||||||
// MUST be enabled for Secure Simple Pairing: IO Capability Request
|
// MUST be enabled for Secure Simple Pairing: IO Capability Request
|
||||||
// (0x31, bit 48), IO Capability Response (0x32), User Confirmation
|
// (0x31, bit 48), IO Capability Response (0x32), User Confirmation
|
||||||
@@ -261,6 +278,19 @@ namespace Drivers::USB::Bluetooth {
|
|||||||
CompleteInit();
|
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
|
// Public queries
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
|
|||||||
@@ -19,6 +19,14 @@ namespace Drivers::USB::Bluetooth {
|
|||||||
// once the boot filesystems are up; a no-op if nothing is pending.
|
// once the boot filesystems are up; a no-op if nothing is pending.
|
||||||
void ServiceDeferredInit();
|
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
|
// Query adapter state
|
||||||
bool IsInitialized();
|
bool IsInitialized();
|
||||||
uint8_t GetSlotId();
|
uint8_t GetSlotId();
|
||||||
|
|||||||
@@ -6,6 +6,7 @@
|
|||||||
|
|
||||||
#include "Hci.hpp"
|
#include "Hci.hpp"
|
||||||
#include "L2cap.hpp"
|
#include "L2cap.hpp"
|
||||||
|
#include <atomic>
|
||||||
#include <Fs/Vfs.hpp>
|
#include <Fs/Vfs.hpp>
|
||||||
#include <Drivers/USB/Xhci.hpp>
|
#include <Drivers/USB/Xhci.hpp>
|
||||||
#include <Drivers/USB/UsbDevice.hpp>
|
#include <Drivers/USB/UsbDevice.hpp>
|
||||||
@@ -64,13 +65,21 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
// ACL buffer size (from controller)
|
// ACL buffer size (from controller)
|
||||||
static uint16_t g_aclMaxLen = 0;
|
static uint16_t g_aclMaxLen = 0;
|
||||||
static uint16_t g_aclMaxNum = 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<int32_t> g_aclPendingCount{0};
|
||||||
|
|
||||||
// Diagnostic ACL data-path counters: TX submitted, TX completed (bulk OUT
|
// Diagnostic ACL data-path counters: TX submitted, TX completed (bulk OUT
|
||||||
// completion), RX received (bulk IN). Used to tell whether L2CAP signaling
|
// completion), RX received (bulk IN). Used to tell whether L2CAP signaling
|
||||||
// actually flows over ACL after encryption is enabled.
|
// actually flows over ACL after encryption is enabled. tx - txDone is also
|
||||||
static volatile uint32_t g_aclTxCount = 0;
|
// the number of TX DMA ring slots still owned by the xHCI (see AclTxReady).
|
||||||
static volatile uint32_t g_aclTxDoneCount = 0;
|
static std::atomic<uint32_t> g_aclTxCount{0};
|
||||||
|
static std::atomic<uint32_t> g_aclTxDoneCount{0};
|
||||||
static volatile uint32_t g_aclRxCount = 0;
|
static volatile uint32_t g_aclRxCount = 0;
|
||||||
// Incremented when an incoming ACL packet is dropped because the RX ring was
|
// 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
|
// 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);
|
Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket);
|
||||||
}
|
}
|
||||||
} else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) {
|
} else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) {
|
||||||
// Bulk OUT completion — decrement pending count
|
// Bulk OUT completion: the packet was DMA'd to the controller and
|
||||||
g_aclTxDoneCount++;
|
// its TX ring slot is free again. Controller buffer credits
|
||||||
if (g_aclPendingCount > 0) g_aclPendingCount--;
|
// (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;
|
uint32_t totalLen = sizeof(AclHeader) + len;
|
||||||
|
|
||||||
g_aclPendingCount++;
|
g_aclPendingCount.fetch_add(1, std::memory_order_relaxed);
|
||||||
g_aclTxCount++;
|
g_aclTxCount.fetch_add(1, std::memory_order_relaxed);
|
||||||
Xhci::QueueBulkOutTransfer(g_slotId, txBuf, txPhys, totalLen);
|
Xhci::QueueBulkOutTransfer(g_slotId, txBuf, txPhys, totalLen);
|
||||||
|
|
||||||
return true;
|
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; }
|
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() {
|
void DumpAclStats() {
|
||||||
KernelLogStream(INFO, "BT-HCI") << "ACL stats: tx=" << (uint64_t)g_aclTxCount
|
KernelLogStream(INFO, "BT-HCI") << "ACL stats: tx="
|
||||||
<< " txDone=" << (uint64_t)g_aclTxDoneCount
|
<< (uint64_t)g_aclTxCount.load(std::memory_order_relaxed)
|
||||||
|
<< " txDone=" << (uint64_t)g_aclTxDoneCount.load(std::memory_order_relaxed)
|
||||||
<< " rx=" << (uint64_t)g_aclRxCount
|
<< " rx=" << (uint64_t)g_aclRxCount
|
||||||
<< " rxDrop=" << (uint64_t)g_aclRxDropCount
|
<< " rxDrop=" << (uint64_t)g_aclRxDropCount
|
||||||
<< " pending=" << (uint64_t)g_aclPendingCount
|
<< " pending=" << (uint64_t)AclPendingCount()
|
||||||
<< " bufNum=" << (uint64_t)g_aclMaxNum;
|
<< " 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++) {
|
for (int i = 0; i < numHandles && (3 + i * 4) < evtParamLen; i++) {
|
||||||
uint16_t completed = (uint16_t)params[3 + i * 4]
|
uint16_t completed = (uint16_t)params[3 + i * 4]
|
||||||
| ((uint16_t)params[4 + i * 4] << 8);
|
| ((uint16_t)params[4 + i * 4] << 8);
|
||||||
if (g_aclPendingCount >= completed) {
|
int32_t v = g_aclPendingCount.fetch_sub(completed,
|
||||||
g_aclPendingCount -= completed;
|
std::memory_order_relaxed) - completed;
|
||||||
} else {
|
if (v < 0) g_aclPendingCount.store(0, std::memory_order_relaxed);
|
||||||
g_aclPendingCount = 0;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@@ -1070,6 +1109,13 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
// Top-level only: drains queued pairing replies with real, confirmed
|
// Top-level only: drains queued pairing replies with real, confirmed
|
||||||
// control transfers. Must NOT be called from inside PollEvents.
|
// control transfers. Must NOT be called from inside PollEvents.
|
||||||
if (Xhci::InPollContext()) return;
|
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<bool> s_active{false};
|
||||||
|
bool expected = false;
|
||||||
|
if (!s_active.compare_exchange_strong(expected, true,
|
||||||
|
std::memory_order_acquire)) return;
|
||||||
while (g_pendingTail != g_pendingHead) {
|
while (g_pendingTail != g_pendingHead) {
|
||||||
PendingHciCmd c = g_pending[g_pendingTail];
|
PendingHciCmd c = g_pending[g_pendingTail];
|
||||||
g_pendingTail = (uint8_t)((g_pendingTail + 1) & 15);
|
g_pendingTail = (uint8_t)((g_pendingTail + 1) & 15);
|
||||||
@@ -1082,6 +1128,7 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
WaitCommandComplete(c.opcode, nullptr, 0, 1000);
|
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) {
|
bool WaitSecureSendResult(uint32_t timeoutMs, uint8_t* outResult, uint8_t* outStatus) {
|
||||||
@@ -1316,6 +1363,18 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
// =========================================================================
|
// =========================================================================
|
||||||
|
|
||||||
void DrainEvents() {
|
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<bool> 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
|
// Discard any unconsumed Command Complete/Status events that weren't
|
||||||
// picked up by WaitCommandComplete/WaitCommandStatus.
|
// picked up by WaitCommandComplete/WaitCommandStatus.
|
||||||
if (g_eventReady) {
|
if (g_eventReady) {
|
||||||
@@ -1349,6 +1408,8 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
ProcessAcl(g_aclRxRing[slot], pl);
|
ProcessAcl(g_aclRxRing[slot], pl);
|
||||||
g_aclRxTail = (uint8_t)((g_aclRxTail + 1) % ACL_RX_SLOTS);
|
g_aclRxTail = (uint8_t)((g_aclRxTail + 1) % ACL_RX_SLOTS);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
s_draining.store(false, std::memory_order_release);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CreateConnection(const uint8_t* bdAddr) {
|
bool CreateConnection(const uint8_t* bdAddr) {
|
||||||
|
|||||||
@@ -310,6 +310,19 @@ namespace Drivers::USB::Bluetooth::Hci {
|
|||||||
uint16_t AclPendingCount();
|
uint16_t AclPendingCount();
|
||||||
uint16_t AclMaxPackets();
|
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
|
// Disconnect a connection
|
||||||
bool Disconnect(uint16_t handle, uint8_t reason);
|
bool Disconnect(uint16_t handle, uint8_t reason);
|
||||||
|
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
#include "L2cap.hpp"
|
#include "L2cap.hpp"
|
||||||
#include "Hci.hpp"
|
#include "Hci.hpp"
|
||||||
#include "A2dp.hpp"
|
#include "A2dp.hpp"
|
||||||
|
#include "Avrcp.hpp"
|
||||||
#include <Drivers/USB/Xhci.hpp>
|
#include <Drivers/USB/Xhci.hpp>
|
||||||
#include <Terminal/Terminal.hpp>
|
#include <Terminal/Terminal.hpp>
|
||||||
#include <CppLib/Stream.hpp>
|
#include <CppLib/Stream.hpp>
|
||||||
@@ -151,8 +152,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
|||||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM="
|
KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM="
|
||||||
<< base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid;
|
<< base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid;
|
||||||
|
|
||||||
// Accept connections for AVDTP
|
// Accept connections for AVDTP, SDP, and AVRCP control
|
||||||
if (psm == PSM_AVDTP || psm == PSM_SDP) {
|
if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) {
|
||||||
if (psm == PSM_AVDTP) g_incomingAvdtpReqs++;
|
if (psm == PSM_AVDTP) g_incomingAvdtpReqs++;
|
||||||
auto* ch = AllocChannel(psm);
|
auto* ch = AllocChannel(psm);
|
||||||
if (ch) {
|
if (ch) {
|
||||||
@@ -223,6 +224,19 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
|||||||
break;
|
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;
|
break;
|
||||||
@@ -369,8 +383,27 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
|||||||
break;
|
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;
|
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;
|
sOff += sizeof(SignalHeader) + sigPayloadLen;
|
||||||
}
|
}
|
||||||
@@ -381,7 +414,9 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
|||||||
if (g_channels[i].Psm == PSM_AVDTP) {
|
if (g_channels[i].Psm == PSM_AVDTP) {
|
||||||
A2dp::ProcessAvdtp(payload, l2len);
|
A2dp::ProcessAvdtp(payload, l2len);
|
||||||
} else if (g_channels[i].Psm == PSM_SDP) {
|
} 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;
|
break;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -22,6 +22,7 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
|||||||
// =========================================================================
|
// =========================================================================
|
||||||
|
|
||||||
constexpr uint16_t PSM_SDP = 0x0001; // Service Discovery Protocol
|
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
|
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_CONFIG_RSP = 0x05;
|
||||||
constexpr uint8_t SIG_DISCONN_REQ = 0x06;
|
constexpr uint8_t SIG_DISCONN_REQ = 0x06;
|
||||||
constexpr uint8_t SIG_DISCONN_RSP = 0x07;
|
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_REQ = 0x0A;
|
||||||
constexpr uint8_t SIG_INFO_RSP = 0x0B;
|
constexpr uint8_t SIG_INFO_RSP = 0x0B;
|
||||||
|
|
||||||
|
|||||||
@@ -12,39 +12,51 @@
|
|||||||
namespace Drivers::USB::Bluetooth::Sbc {
|
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
|
// SBC 8-subband prototype window, the 80 coefficients from the A2DP spec
|
||||||
constexpr int32_t FP_ONE = (1 << 15);
|
// (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).
|
||||||
// SBC 8-subband analysis filter prototype coefficients (Q1.30)
|
// Verified on host: feeding subband-center sines concentrates energy in the
|
||||||
// These are the 80 windowed prototype coefficients from the SBC spec
|
// right band with >63 dB rejection and overall gain 0.9997.
|
||||||
// Scaled to Q15.16 for our fixed-point arithmetic
|
|
||||||
static const int32_t g_proto8[80] = {
|
static const int32_t g_proto8[80] = {
|
||||||
0x0002, 0x0005, 0x000A, 0x0014, 0x0023, 0x0038, 0x0054, 0x0078,
|
(int32_t)0x00000000, (int32_t)0x00052173, (int32_t)0x000B3F71, (int32_t)0x00122C7D,
|
||||||
0x00A5, 0x00DB, 0x011B, 0x0164, 0x01B5, 0x020E, 0x026D, 0x02D0,
|
(int32_t)0x001AFF89, (int32_t)0x00255A62, (int32_t)0x003060F4, (int32_t)0x003A72E7,
|
||||||
0x0335, 0x039A, 0x03FC, 0x0458, 0x04AB, 0x04F1, 0x0527, 0x054A,
|
(int32_t)0x0041EC6A, (int32_t)0x0044EF48, (int32_t)0x00415B75, (int32_t)0x0034F8B6,
|
||||||
0x0557, 0x054A, 0x0527, 0x04F1, 0x04AB, 0x0458, 0x03FC, 0x039A,
|
(int32_t)0x001D8FD2, (int32_t)0xFFFA2413, (int32_t)0xFFC9F10E, (int32_t)0xFF8D6793,
|
||||||
0x0335, 0x02D0, 0x026D, 0x020E, 0x01B5, 0x0164, 0x011B, 0x00DB,
|
(int32_t)0x00B97348, (int32_t)0x01071B96, (int32_t)0x0156B3CA, (int32_t)0x01A1B38B,
|
||||||
0x00A5, 0x0078, 0x0054, 0x0038, 0x0023, 0x0014, 0x000A, 0x0005,
|
(int32_t)0x01E0224C, (int32_t)0x0209291F, (int32_t)0x02138653, (int32_t)0x01F5F424,
|
||||||
0x0002, -0x0002, -0x0005, -0x000A, -0x0014, -0x0023, -0x0038, -0x0054,
|
(int32_t)0x01A7ECEF, (int32_t)0x01223EBA, (int32_t)0x005FD0FF, (int32_t)0xFF5EEB73,
|
||||||
-0x0078, -0x00A5, -0x00DB, -0x011B, -0x0164, -0x01B5, -0x020E, -0x026D,
|
(int32_t)0xFE20435D, (int32_t)0xFCA86E7E, (int32_t)0xFAFF95FC, (int32_t)0xF9312891,
|
||||||
-0x02D0, -0x0335, -0x039A, -0x03FC, -0x0458, -0x04AB, -0x04F1, -0x0527,
|
(int32_t)0x08B4307A, (int32_t)0x0A9F3E9A, (int32_t)0x0C7D59B6, (int32_t)0x0E3BB16F,
|
||||||
-0x054A, -0x0557, -0x054A, -0x0527, -0x04F1, -0x04AB, -0x0458, -0x03FC,
|
(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)
|
// Analysis matrixing for 8 subbands (A2DP spec Annex B), Q14:
|
||||||
// cos_matrix[k][i] = cos((k + 0.5) * (2*i + 1) * PI / 16) * FP_ONE
|
// M[k][i] = cos((i - 4) * (2k + 1) * pi / 16) * 2^14
|
||||||
static const int32_t g_cosMatrix8[8][16] = {
|
// (The (i-4) phase is the one whose aliasing cancels in the reference
|
||||||
{ 32138, 31650, 30679, 29246, 27381, 25126, 22529, 19644, 16531, 13254, 9882, 6484, 3134, -199, -3509, -6758},
|
// decoder's synthesis; (i+4) passes an energy-concentration test equally
|
||||||
{ 30679, 25126, 16531, 6484, -3509,-13254,-22529,-29246,-32138,-31650,-27381,-19644, -9882, 199, 9882, 19644},
|
// but reconstructs with frequency-dependent distortion.)
|
||||||
{ 27381, 13254, -3509,-19644,-30679,-32138,-22529, -6484, 9882, 25126, 32138, 29246, 16531, -199,-16531,-29246},
|
static const int16_t g_cosMatrix8[8][16] = {
|
||||||
{ 22529, -199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199},
|
{ 11585, 13623, 15137, 16069, 16384, 16069, 15137, 13623, 11585, 9102, 6270, 3196, 0, -3196, -6270, -9102 },
|
||||||
{ 16531,-13254,-30679, 6484, 32138, -199,-32138, -6484, 30679, 13254,-16531,-25126, 3509, 29246, 9882,-27381},
|
{ -11585, -3196, 6270, 13623, 16384, 13623, 6270, -3196, -11585, -16069, -15137, -9102, 0, 9102, 15137, 16069 },
|
||||||
{ 9882,-25126,-16531, 29246, 3509,-32138, 9882, 25126,-16531,-29246, 3509, 32138, -9882,-25126, 16531, 29246},
|
{ -11585, -16069, -6270, 9102, 16384, 9102, -6270, -16069, -11585, 3196, 15137, 13623, 0, -13623, -15137, -3196 },
|
||||||
{ 3134,-31650, 27381, -6484,-22529, 32138,-13254, -9882, 30679,-29246, 6484, 19644,-32138, 16531, 3509,-25126},
|
{ 11585, -9102, -15137, 3196, 16384, 3196, -15137, -9102, 11585, 13623, -6270, -16069, 0, 16069, 6270, -13623 },
|
||||||
{ -3509, 32138,-22529, -6484, 30679,-27381, 3509, 25126,-32138, 16531, 9882,-30679, 22529, -199,-25126, 32138},
|
{ 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)
|
// 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,
|
static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch,
|
||||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) {
|
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) {
|
||||||
|
int32_t* X = enc->X[ch];
|
||||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||||
// Shift in new samples
|
for (int i = 79; i >= 8; i--) X[i] = X[i - 8];
|
||||||
int pos = enc->XPos[ch];
|
const int16_t* in = pcm + blk * 8 * enc->Channels + ch;
|
||||||
for (int i = enc->Subbands - 1; i >= 0; i--) {
|
for (int i = 0; i < 8; i++) {
|
||||||
pos = (pos + 1) % (enc->Subbands * 10);
|
X[i] = (int32_t)in[(7 - i) * enc->Channels];
|
||||||
enc->X[ch][pos] = (int32_t)pcm[blk * enc->Subbands * enc->Channels + i * enc->Channels + ch];
|
|
||||||
}
|
}
|
||||||
enc->XPos[ch] = pos;
|
|
||||||
|
|
||||||
// Windowing and partial calculation
|
int32_t Y[16];
|
||||||
int32_t Z[2 * SBC_SUBBANDS];
|
for (int i = 0; i < 16; i++) {
|
||||||
for (int i = 0; i < 2 * enc->Subbands; i++) {
|
int64_t acc = 0;
|
||||||
Z[i] = 0;
|
|
||||||
for (int j = 0; j < 5; j++) {
|
for (int j = 0; j < 5; j++) {
|
||||||
int idx = (pos + i + j * 2 * enc->Subbands) % (enc->Subbands * 10);
|
acc += (int64_t)X[i + 16 * j] * g_proto8[i + 16 * j];
|
||||||
int protoIdx = i + j * 2 * enc->Subbands;
|
|
||||||
if (protoIdx < 80) {
|
|
||||||
Z[i] += (int32_t)(((int64_t)enc->X[ch][idx] * g_proto8[protoIdx]) >> 15);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
Y[i] = (int32_t)((acc + (1 << 15)) >> 16);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Matrixing (DCT)
|
for (int k = 0; k < 8; k++) {
|
||||||
for (int k = 0; k < enc->Subbands; k++) {
|
int64_t s = 0;
|
||||||
int32_t sum = 0;
|
for (int i = 0; i < 16; i++) {
|
||||||
for (int i = 0; i < 2 * enc->Subbands; i++) {
|
s += (int64_t)Y[i] * g_cosMatrix8[k][i];
|
||||||
sum += (int32_t)(((int64_t)Z[i] * g_cosMatrix8[k][i]) >> 15);
|
|
||||||
}
|
}
|
||||||
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 < 0) val = -val;
|
||||||
if (val > maxVal) maxVal = 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; }
|
while (tmp > 0) { sf++; tmp >>= 1; }
|
||||||
scale_factors[sb] = sf;
|
scale_factors[sb] = sf;
|
||||||
}
|
}
|
||||||
@@ -344,40 +369,48 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
|||||||
AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
|
AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. Joint-stereo decision + mid/side transform, applied BEFORE scale
|
// 2. Scale factors from the subband samples.
|
||||||
// factors and bit allocation: the decoder re-derives the bit widths
|
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||||
// from the transmitted scale factors, which must reflect the FINAL
|
ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]);
|
||||||
// (post-transform) samples, or the sample bitstream desyncs.
|
}
|
||||||
|
|
||||||
|
// 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;
|
uint8_t joint = 0;
|
||||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||||
for (int sb = 0; sb < enc->Subbands - 1; sb++) {
|
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++) {
|
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||||
int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb];
|
int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb];
|
||||||
int32_t mid = (l + r) / 2, side = (l - r) / 2;
|
mid[blk] = (l >> 1) + (r >> 1);
|
||||||
if (mid < 0) mid = -mid;
|
side[blk] = (l >> 1) - (r >> 1);
|
||||||
if (side < 0) side = -side;
|
int32_t am = mid[blk] < 0 ? -mid[blk] : mid[blk];
|
||||||
int32_t al = l < 0 ? -l : l, ar = r < 0 ? -r : r;
|
int32_t as = side[blk] < 0 ? -side[blk] : side[blk];
|
||||||
if (mid > maxMid) maxMid = mid;
|
if (am > maxMid) maxMid = am;
|
||||||
if (side > maxSide) maxSide = side;
|
if (as > maxSide) maxSide = as;
|
||||||
if (al > maxOrig) maxOrig = al;
|
|
||||||
if (ar > maxOrig) maxOrig = ar;
|
|
||||||
}
|
|
||||||
if (maxMid + maxSide < maxOrig) {
|
|
||||||
joint |= (1 << (enc->Subbands - 1 - sb));
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
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; }
|
||||||
|
|
||||||
// 3. Scale factors from the final subband samples.
|
if (scale_factors[0][sb] + scale_factors[1][sb] > sfm + sfs) {
|
||||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
joint |= (uint8_t)(1 << (enc->Subbands - 1 - sb));
|
||||||
ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]);
|
scale_factors[0][sb] = sfm;
|
||||||
|
scale_factors[1][sb] = sfs;
|
||||||
|
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||||
|
sb_samples[0][blk][sb] = mid[blk];
|
||||||
|
sb_samples[1][blk][sb] = side[blk];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 4. Bit allocation (canonical; decoder re-derives the same widths).
|
// 4. Bit allocation (canonical; decoder re-derives the same widths).
|
||||||
@@ -434,13 +467,13 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
|||||||
int32_t sf = scale_factors[ch][sb];
|
int32_t sf = scale_factors[ch][sb];
|
||||||
int32_t sample = sb_samples[ch][blk][sb];
|
int32_t sample = sb_samples[ch][blk][sb];
|
||||||
uint32_t levels = (1u << bits[ch][sb]) - 1;
|
uint32_t levels = (1u << bits[ch][sb]) - 1;
|
||||||
int32_t quantized;
|
// q = levels * (sample + range) / (2 * range), truncating,
|
||||||
if (sf > 0) {
|
// with range = 2^(sf + FRAC_BITS) -- same mapping as the
|
||||||
int32_t maxRange = (1 << sf);
|
// reference encoder. The fractional sample bits feed
|
||||||
quantized = (int32_t)(((int64_t)(sample + maxRange) * levels) / (2 * maxRange));
|
// straight into the quantizer's precision.
|
||||||
} else {
|
int32_t range = 1 << (sf + FRAC_BITS);
|
||||||
quantized = levels / 2;
|
int32_t quantized = (int32_t)
|
||||||
}
|
(((int64_t)(sample + range) * levels) >> (sf + FRAC_BITS + 1));
|
||||||
if (quantized < 0) quantized = 0;
|
if (quantized < 0) quantized = 0;
|
||||||
if (quantized > (int32_t)levels) quantized = (int32_t)levels;
|
if (quantized > (int32_t)levels) quantized = (int32_t)levels;
|
||||||
WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
|
WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
|
||||||
|
|||||||
@@ -57,9 +57,9 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
|||||||
uint8_t Bitpool;
|
uint8_t Bitpool;
|
||||||
uint8_t Channels;
|
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];
|
int32_t X[SBC_CHANNELS][SBC_SUBBANDS * 10];
|
||||||
int XPos[SBC_CHANNELS];
|
|
||||||
|
|
||||||
// Computed frame size in bytes
|
// Computed frame size in bytes
|
||||||
uint32_t FrameSize;
|
uint32_t FrameSize;
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "Xhci.hpp"
|
#include "Xhci.hpp"
|
||||||
|
#include <atomic>
|
||||||
#include "UsbDevice.hpp"
|
#include "UsbDevice.hpp"
|
||||||
#include "HidKeyboard.hpp"
|
#include "HidKeyboard.hpp"
|
||||||
#include "HidMouse.hpp"
|
#include "HidMouse.hpp"
|
||||||
@@ -112,7 +113,17 @@ namespace Drivers::USB::Xhci {
|
|||||||
// Bluetooth event handler) see this and must NOT wait for completion --
|
// Bluetooth event handler) see this and must NOT wait for completion --
|
||||||
// PollEvents is non-reentrant, so the wait would never observe the
|
// PollEvents is non-reentrant, so the wait would never observe the
|
||||||
// completion. Used by InPollContext() / ControlTransfer().
|
// 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<bool> g_pollActive{false};
|
||||||
|
|
||||||
// Interrupt transfer data buffers (per slot)
|
// Interrupt transfer data buffers (per slot)
|
||||||
static uint8_t* g_interruptDataBuf[MAX_SLOTS + 1] = {};
|
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,
|
// sent from a Bluetooth event handler). Such callers must fire-and-forget,
|
||||||
// not wait, since a nested PollEvents is a no-op.
|
// not wait, since a nested PollEvents is a no-op.
|
||||||
bool InPollContext() {
|
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
|
// the active poll loop drains these events itself. g_pollActive is also
|
||||||
// read by InPollContext() so command submitters (ControlTransfer) can
|
// read by InPollContext() so command submitters (ControlTransfer) can
|
||||||
// tell they are nested and must fire-and-forget instead of waiting.
|
// tell they are nested and must fire-and-forget instead of waiting.
|
||||||
if (g_pollActive) return;
|
bool expected = false;
|
||||||
g_pollActive = true;
|
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
|
// Bound the work per call so a flooding/wedged device can never spin
|
||||||
// here forever; the outer wall-clock timeouts then fire instead of
|
// 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
|
// would never be observed and the timeout+recovery path would corrupt
|
||||||
// the EP0 ring. The transfer is submitted (doorbell rung); let the
|
// the EP0 ring. The transfer is submitted (doorbell rung); let the
|
||||||
// active PollEvents reap its completion. Fire-and-forget.
|
// active PollEvents reap its completion. Fire-and-forget.
|
||||||
if (g_pollActive) {
|
if (g_pollActive.load(std::memory_order_relaxed)) {
|
||||||
return CC_SUCCESS;
|
return CC_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -17,6 +17,7 @@
|
|||||||
#include <Sched/Scheduler.hpp>
|
#include <Sched/Scheduler.hpp>
|
||||||
#include <Drivers/Net/E1000E.hpp>
|
#include <Drivers/Net/E1000E.hpp>
|
||||||
#include <Drivers/USB/Xhci.hpp>
|
#include <Drivers/USB/Xhci.hpp>
|
||||||
|
#include <Drivers/USB/Bluetooth/Bluetooth.hpp>
|
||||||
#include <Drivers/USB/HidKeyboard.hpp>
|
#include <Drivers/USB/HidKeyboard.hpp>
|
||||||
|
|
||||||
using namespace Kt;
|
using namespace Kt;
|
||||||
@@ -211,6 +212,15 @@ namespace Timekeeping {
|
|||||||
Drivers::USB::Xhci::ProcessDeferredWork();
|
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) {
|
if (cpu == nullptr || cpu->cpuIndex != 0 || !g_schedEnabled || g_ticksPerMs == 0) {
|
||||||
// Non-BSP or pre-scheduler fallback: plain HLT keeps the LAPIC
|
// Non-BSP or pre-scheduler fallback: plain HLT keeps the LAPIC
|
||||||
// timer running in C1 on hardware where MWAIT promotes to deeper
|
// timer running in C1 on hardware where MWAIT promotes to deeper
|
||||||
|
|||||||
Reference in New Issue
Block a user