Asrc: Refactor the clock recovery to use HCI Read Clock information as source of truth (cbe8d23d) · Commits · e / os / android_packages_modules_Bluetooth

system/audio/Android.bp

+35 −3

Original line number	Original line	Diff line number	Diff line
	@@ -14,10 +14,26 @@ cc_library_static {
	"asrc/asrc_resampler.cc",		"asrc/asrc_resampler.cc",
	"asrc/asrc_tables.cc",		"asrc/asrc_tables.cc",
	],		],
			include_dirs: [
			"packages/modules/Bluetooth/system",
			"packages/modules/Bluetooth/system/bta/include",
			"packages/modules/Bluetooth/system/btif/avrcp",
			"packages/modules/Bluetooth/system/gd",
			"packages/modules/Bluetooth/system/stack/btm",
			"packages/modules/Bluetooth/system/stack/include",
			"packages/modules/Bluetooth/system/udrv/include",
			],
			header_libs: [
			"libbluetooth_headers",
			],
	shared_libs: [		shared_libs: [
	"libchrome",		"libchrome",
	],		],
	static_libs: [		static_libs: [
			"libbase",
			"libbluetooth_hci_pdl",
			"libbluetooth_log",
			"libbt_shim_bridge",
	"libflatbuffers-cpp",		"libflatbuffers-cpp",
	],		],
	host_supported: true,		host_supported: true,
	@@ -31,17 +47,33 @@ cc_library_host_shared {
	name: "libasrc_resampler_test",		name: "libasrc_resampler_test",
	defaults: ["bluetooth_cflags"],		defaults: ["bluetooth_cflags"],
	srcs: [		srcs: [
			":TestMockMainShimEntry",
	"asrc/asrc_resampler_test.cc",		"asrc/asrc_resampler_test.cc",
	"asrc/asrc_tables.cc",		"asrc/asrc_tables.cc",
	],		],
			include_dirs: [
			"packages/modules/Bluetooth/system",
			"packages/modules/Bluetooth/system/bta/include",
			"packages/modules/Bluetooth/system/btif/avrcp",
			"packages/modules/Bluetooth/system/gd",
			"packages/modules/Bluetooth/system/stack/btm",
			"packages/modules/Bluetooth/system/stack/include",
			"packages/modules/Bluetooth/system/udrv/include",
			],
			header_libs: [
			"libbluetooth_headers",
			],
	static_libs: [		static_libs: [
			"libbluetooth_hci_pdl",
			"libbluetooth_log",
			"libbt-common",
			"libbt_shim_bridge",
	"libchrome",		"libchrome",
			"libevent",
	"libflatbuffers-cpp",		"libflatbuffers-cpp",
			"libgmock",
	],		],
	stl: "libc++_static",		stl: "libc++_static",
	include_dirs: [
	"packages/modules/Bluetooth/system",
	],
	generated_headers: [		generated_headers: [
	"BluetoothGeneratedDumpsysDataSchema_h",		"BluetoothGeneratedDumpsysDataSchema_h",
	],		],

system/audio/asrc/asrc_resampler.cc

+73 −123

Original line number	Original line	Diff line number	Diff line
	@@ -24,36 +24,32 @@
	#include <utility>		#include <utility>

	#include "asrc_tables.h"		#include "asrc_tables.h"
			#include "common/repeating_timer.h"
			#include "hal/link_clocker.h"
			#include "hci/hci_layer.h"
			#include "hci/hci_packets.h"
			#include "main/shim/entry.h"
			#include "stack/include/main_thread.h"

	namespace bluetooth::audio::asrc {		namespace bluetooth::audio::asrc {

	class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {		class SourceAudioHalAsrc::ClockRecovery
	const int interval_;		: public bluetooth::hal::ReadClockHandler {

	std::mutex mutex_;		std::mutex mutex_;
			bluetooth::common::RepeatingTimer read_clock_timer_;

	unsigned num_produced_;		enum class StateId { RESET, WARMUP, RUNNING };

	enum class LinkState { RESET, WARMUP, RUNNING };

	struct {		struct {
	struct {		StateId id;
	LinkState state;

	uint32_t local_time;
	uint32_t decim_t0;
	int decim_dt[2];

	unsigned num_completed;
	int min_buffer_level;

	} link[2];

	int active_link_id;

	uint32_t t0;		uint32_t t0;
	uint32_t local_time;		uint32_t local_time;
	uint32_t stream_time;		uint32_t stream_time;
			uint32_t last_bt_clock;

			uint32_t decim_t0;
			int decim_dt[2];

	double butter_drift;		double butter_drift;
	double butter_s[2];		double butter_s[2];
	@@ -71,109 +67,62 @@ class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
	} output_stats_;		} output_stats_;

	__attribute__((no_sanitize("integer"))) void OnEvent(		__attribute__((no_sanitize("integer"))) void OnEvent(
	uint32_t timestamp_us, int link_id, int num_completed) override {		uint32_t timestamp_us, uint32_t bt_clock) override {
	auto& state = state_;		auto& state = state_;
	auto& link = state.link[link_id];

	// Setup the start point of the streaming		// Setup the start point of the streaming

	if (link.state == LinkState::RESET) {		if (state.id == StateId::RESET) {
	if (state.link[link_id ^ 1].state == LinkState::RESET) {
	state.t0 = timestamp_us;		state.t0 = timestamp_us;
	state.local_time = timestamp_us;		state.local_time = state.stream_time = state.t0;
	}		state.last_bt_clock = bt_clock;

	link.local_time = timestamp_us;
	link.decim_t0 = timestamp_us;
	link.decim_dt[1] = INT_MAX;

	link.num_completed = 0;
	link.min_buffer_level = INT_MAX;

	link.state = LinkState::WARMUP;
	}

	// Update buffering level measure

	{		state.decim_t0 = state.t0;
	const std::lock_guard<std::mutex> lock(mutex_);		state.decim_dt[1] = INT_MAX;

	link.num_completed += num_completed;		state.id = StateId::WARMUP;
	link.min_buffer_level = std::min(link.min_buffer_level,
	int(num_produced_ - link.num_completed));
	}		}

	// Update timing informations, and compute the minimum deviation		// Update timing informations, and compute the minimum deviation
	// in the interval of the decimation (1 second).		// in the interval of the decimation (1 second).

	link.local_time += num_completed * interval_;		// Convert the local clock interval from the last subampling event
	if (link_id == state.active_link_id) {		// into microseconds.
	state.local_time += num_completed * interval_;		uint32_t elapsed_us = ((bt_clock - state.last_bt_clock) * 625) >> 5;
	state.stream_time += num_completed * interval_;
	}

	int dt_current = int(timestamp_us - link.local_time);		uint32_t local_time = state.local_time + elapsed_us;
	link.decim_dt[1] = std::min(link.decim_dt[1], dt_current);		int dt_current = int(timestamp_us - local_time);
			state.decim_dt[1] = std::min(state.decim_dt[1], dt_current);

	if (link.local_time - link.decim_t0 < 1000 * 1000) return;		if (local_time - state.decim_t0 < 1000 * 1000) return;

	link.decim_t0 += 1000 * 1000;		state.decim_t0 += 1000 * 1000;

			state.last_bt_clock = bt_clock;
			state.local_time += elapsed_us;
			state.stream_time += elapsed_us;

	// The first decimation interval is used to adjust the start point.		// The first decimation interval is used to adjust the start point.
	// The deviation between local time and stream time in this interval can be		// The deviation between local time and stream time in this interval can be
	// ignored.		// ignored.

	if (link.state == LinkState::WARMUP) {		if (state.id == StateId::WARMUP) {
	link.decim_t0 += link.decim_dt[1];		state.decim_t0 += state.decim_dt[1];
	link.local_time += link.decim_dt[1];		state.local_time += state.decim_dt[1];
	if (state.active_link_id < 0) {		state.stream_time += state.decim_dt[1];
	state.active_link_id = link_id;
	state.local_time = link.local_time;
	state.stream_time = link.local_time;
	}

	link.decim_dt[0] = 0;		state.decim_dt[0] = 0;
	link.decim_dt[1] = INT_MAX;		state.decim_dt[1] = INT_MAX;
	link.state = LinkState::RUNNING;		state.id = StateId::RUNNING;
	return;		return;
	}		}

	// Deduct the derive of the deviation, from the difference between		// Deduct the derive of the deviation, from the difference between
	// the two consecutives decimated deviations.		// the two consecutives decimated deviations.

	int drift = link.decim_dt[1] - link.decim_dt[0];		int drift = state.decim_dt[1] - state.decim_dt[0];
	link.decim_dt[0] = link.decim_dt[1];		state.decim_dt[0] = state.decim_dt[1];
	link.decim_dt[1] = INT_MAX;		state.decim_dt[1] = INT_MAX;

	// Sanity check, limit the instant derive to +/- 50 ms / second,
	// and the gap between the 2 links to +/- 250ms. Reset the link state
	// with out of range drift, and eventually active the other link.

	int dt_link = state.link[link_id ^ 1].state == LinkState::RUNNING
	? link.local_time - state.link[link_id ^ 1].local_time
	: 0;
	bool stalled = std::abs(dt_link) > 250 * 1000;

	if (std::abs(drift) > 50 * 1000 \|\| stalled) {
	int bad_link_id = link_id ^ stalled;
	auto& bad_link = state.link[bad_link_id];

	bool resetting = (bad_link.state != LinkState::RUNNING);
	bool switching =
	state.active_link_id == bad_link_id &&
	(state.link[bad_link_id ^ 1].state == LinkState::RUNNING);

	bad_link.state = LinkState::RESET;
	if (bad_link_id == state.active_link_id) state.active_link_id = -1;

	if (switching) state.active_link_id = bad_link_id ^ 1;

	if (resetting \|\| switching)
	LOG(WARNING) << "Link unstable or stalled, "
	<< (switching ? "switching" : "resetting") << std::endl;
	}

	if (link_id != state.active_link_id) return;

	// Let's filter the derive, with a low-pass Butterworth filter.		// Let's filter the derive, with a low-pass Butterworth filter.
	// The cut-off frequency is set to 1/60th seconds.		// The cut-off frequency is set to 1/60th seconds.
	@@ -191,14 +140,13 @@ class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
	// the difference between the instant stream time, and the local time		// the difference between the instant stream time, and the local time
	// corrected by the decimated deviation.		// corrected by the decimated deviation.

	int err = state.stream_time - (state.local_time + link.decim_dt[0]);		int err = state.stream_time - (state.local_time + state.decim_dt[0]);
	state.stream_time +=		state.stream_time +=
	(int(ldexpf(state.butter_drift, 8)) - err + (1 << 7)) >> 8;		(int(ldexpf(state.butter_drift, 8)) - err + (1 << 7)) >> 8;

	// Update recovered timing information, and sample the output statistics.		// Update recovered timing information, and sample the output statistics.

	decltype(output_stats_) output_stats;		decltype(output_stats_) output_stats;
	int min_buffer_level;

	{		{
	const std::lock_guard<std::mutex> lock(mutex_);		const std::lock_guard<std::mutex> lock(mutex_);
	@@ -218,27 +166,31 @@ class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
	<< base::StringPrintf("Output Fs: %5.2f Hz drift: %2d us",		<< base::StringPrintf("Output Fs: %5.2f Hz drift: %2d us",
	output_stats.sample_rate,		output_stats.sample_rate,
	output_stats.drift_us)		output_stats.drift_us)
	<< " \| "
	<< base::StringPrintf(
	"Buffer level: %d:%d",
	std::min(state.link[0].min_buffer_level, 99),
	std::min(state.link[1].min_buffer_level, 99))
	<< std::endl;		<< std::endl;

	state.link[0].min_buffer_level = INT_MAX;
	state.link[1].min_buffer_level = INT_MAX;
	}		}

	public:		public:
	ClockRecovery(unsigned interval_us)		ClockRecovery() : state_{.id = StateId::RESET}, reference_timing_{0, 0, 0} {
	: interval_(interval_us),		read_clock_timer_.SchedulePeriodic(
	num_produced_(0),		get_main_thread()->GetWeakPtr(), FROM_HERE,
	state_{		base::BindRepeating(
	.link = {{.state = LinkState::RESET}, {.state = LinkState::RESET}},		[](void*) {
	.active_link_id = -1},		bluetooth::shim::GetHciLayer()->EnqueueCommand(
	reference_timing_{0, 0, 0} {}		bluetooth::hci::ReadClockBuilder::Create(
			0, bluetooth::hci::WhichClock::LOCAL),
			get_main_thread()->BindOnce(
			[](bluetooth::hci::CommandCompleteView) {}));
			},
			nullptr),
			std::chrono::milliseconds(100));

			hal::LinkClocker::Register(this);
			}

	~ClockRecovery() override {}		~ClockRecovery() override {
			hal::LinkClocker::Unregister();
			read_clock_timer_.Cancel();
			}

	__attribute__((no_sanitize("integer"))) uint32_t Convert(		__attribute__((no_sanitize("integer"))) uint32_t Convert(
	uint32_t stream_time) {		uint32_t stream_time) {
	@@ -254,13 +206,12 @@ class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
	return ref.local_time + local_dt_us;		return ref.local_time + local_dt_us;
	}		}

	void UpdateOutputStats(unsigned out_count, double sample_rate, int drift_us) {		void UpdateOutputStats(double sample_rate, int drift_us) {
	// Atomically update the output statistics,		// Atomically update the output statistics,
	// this should be used for logging.		// this should be used for logging.

	const std::lock_guard<std::mutex> lock(mutex_);		const std::lock_guard<std::mutex> lock(mutex_);

	num_produced_ += out_count;
	output_stats_ = {sample_rate, drift_us};		output_stats_ = {sample_rate, drift_us};
	}		}
	};		};
	@@ -463,16 +414,16 @@ inline int32_t SourceAudioHalAsrc::Resampler::Filter(const int32_t* in,

	#endif		#endif

	SourceAudioHalAsrc::SourceAudioHalAsrc(		SourceAudioHalAsrc::SourceAudioHalAsrc(int channels, int sample_rate,
	std::shared_ptr<ClockSource> clock_source, int channels, int sample_rate,		int bit_depth, int interval_us,
	int bit_depth, int interval_us, int num_burst_buffers, int burst_delay_ms)		int num_burst_buffers,
			int burst_delay_ms)
	: sample_rate_(sample_rate),		: sample_rate_(sample_rate),
	bit_depth_(bit_depth),		bit_depth_(bit_depth),
	interval_us_(interval_us),		interval_us_(interval_us),
	stream_us_(0),		stream_us_(0),
	drift_us_(0),		drift_us_(0),
	out_counter_(0),		out_counter_(0),
	clock_source_(std::move(clock_source)),
	resampler_pos_{0, 0} {		resampler_pos_{0, 0} {
	buffers_size_ = 0;		buffers_size_ = 0;

	@@ -505,8 +456,7 @@ SourceAudioHalAsrc::SourceAudioHalAsrc(
	// Setup modules, the 32 bits resampler is choosed over the 16 bits resampler		// Setup modules, the 32 bits resampler is choosed over the 16 bits resampler
	// when the PCM bit_depth is higher than 16 bits.		// when the PCM bit_depth is higher than 16 bits.

	clock_recovery_ = std::make_unique<ClockRecovery>(interval_us_);		clock_recovery_ = std::make_unique<ClockRecovery>();
	clock_source_->Bind(clock_recovery_.get());
	resamplers_ = std::make_unique<std::vector<Resampler>>(channels, bit_depth_);		resamplers_ = std::make_unique<std::vector<Resampler>>(channels, bit_depth_);

	// Deduct from the PCM stream characteristics, the size of the pool buffers		// Deduct from the PCM stream characteristics, the size of the pool buffers
	@@ -657,7 +607,7 @@ SourceAudioHalAsrc::Run(const std::vector<uint8_t>& in) {
	// Return the output statistics to the clock recovery module		// Return the output statistics to the clock recovery module

	out_counter_ += out.size();		out_counter_ += out.size();
	clock_recovery_->UpdateOutputStats(out.size(), ratio * sample_rate_,		clock_recovery_->UpdateOutputStats(ratio * sample_rate_,
	int(output_us - local_us));		int(output_us - local_us));

	if (0)		if (0)

system/audio/asrc/asrc_resampler.h

+3 −17

Original line number	Original line	Diff line number	Diff line
	@@ -23,19 +23,6 @@

	namespace bluetooth::audio::asrc {		namespace bluetooth::audio::asrc {

	class ClockHandler {
	public:
	virtual ~ClockHandler() = default;
	virtual void OnEvent(uint32_t timestamp, int link_id,
	int num_of_completed_packets) = 0;
	};

	class ClockSource {
	public:
	virtual ~ClockSource() = default;
	virtual void Bind(ClockHandler*) = 0;
	};

	class SourceAudioHalAsrc {		class SourceAudioHalAsrc {
	public:		public:
	// The Asynchronous Sample Rate Conversion (ASRC) is set up from the PCM		// The Asynchronous Sample Rate Conversion (ASRC) is set up from the PCM
	@@ -49,9 +36,9 @@ class SourceAudioHalAsrc {
	// `burst_delay_ms` helps to ensure that the synchronization with the		// `burst_delay_ms` helps to ensure that the synchronization with the
	// transmission intervals is done.		// transmission intervals is done.

	SourceAudioHalAsrc(std::shared_ptr<ClockSource> clock_source, int channels,		SourceAudioHalAsrc(int channels, int sample_rate, int bit_depth,
	int sample_rate, int bit_depth, int interval_us,		int interval_us, int num_burst_buffers = 2,
	int num_burst_buffers = 2, int burst_delay_ms = 500);		int burst_delay_ms = 500);

	~SourceAudioHalAsrc();		~SourceAudioHalAsrc();

	@@ -87,7 +74,6 @@ class SourceAudioHalAsrc {

	class ClockRecovery;		class ClockRecovery;
	std::unique_ptr<ClockRecovery> clock_recovery_;		std::unique_ptr<ClockRecovery> clock_recovery_;
	std::shared_ptr<ClockSource> clock_source_;

	class Resampler;		class Resampler;
	std::unique_ptr<std::vector<Resampler>> resamplers_;		std::unique_ptr<std::vector<Resampler>> resamplers_;

system/audio/asrc/asrc_resampler_test.cc

+11 −6

Original line number	Original line	Diff line number	Diff line
	@@ -19,17 +19,22 @@
	#include <cstdio>		#include <cstdio>
	#include <iostream>		#include <iostream>

	namespace bluetooth::audio::asrc {		bluetooth::common::MessageLoopThread message_loop_thread("main message loop");
			bluetooth::common::MessageLoopThread* get_main_thread() {
			return &message_loop_thread;
			}

	class MockClockSource : public ClockSource {		namespace bluetooth::hal {
	void Bind(ClockHandler*) override {}		void LinkClocker::Register(ReadClockHandler*) {}
	};		void LinkClocker::Unregister() {}
			} // namespace bluetooth::hal

			namespace bluetooth::audio::asrc {

	class SourceAudioHalAsrcTest : public SourceAudioHalAsrc {		class SourceAudioHalAsrcTest : public SourceAudioHalAsrc {
	public:		public:
	SourceAudioHalAsrcTest(int channels, int bitdepth)		SourceAudioHalAsrcTest(int channels, int bitdepth)
	: SourceAudioHalAsrc(std::make_unique<MockClockSource>(), channels, 48000,		: SourceAudioHalAsrc(channels, 48000, bitdepth, 10000) {}
	bitdepth, 10000) {}

	template <typename T>		template <typename T>
	void Resample(double ratio, const T* in, size_t in_length, size_t* in_count,		void Resample(double ratio, const T* in, size_t in_length, size_t* in_count,

system/bta/hearing_aid/hearing_aid.cc

+2 −21

Original line number	Original line	Diff line number	Diff line
	@@ -281,7 +281,6 @@ class HearingAidImpl : public HearingAid {
	// Clock recovery uses L2CAP Flow Control Credit Ind acknowledgments		// Clock recovery uses L2CAP Flow Control Credit Ind acknowledgments
	// from either the left or right connection, whichever is first		// from either the left or right connection, whichever is first
	// connected.		// connected.
	std::shared_ptr<bluetooth::hal::L2capCreditIndEvents> asrc_clock_source;
	std::unique_ptr<bluetooth::audio::asrc::SourceAudioHalAsrc> asrc;		std::unique_ptr<bluetooth::audio::asrc::SourceAudioHalAsrc> asrc;

	public:		public:
	@@ -373,38 +372,20 @@ class HearingAidImpl : public HearingAid {

	// Create a new ASRC context if required.		// Create a new ASRC context if required.
	if (asrc == nullptr) {		if (asrc == nullptr) {
	asrc_clock_source =		log::info("Configuring Asha resampler");
	std::make_shared<bluetooth::hal::L2capCreditIndEvents>();
	asrc = std::make_unique<bluetooth::audio::asrc::SourceAudioHalAsrc>(		asrc = std::make_unique<bluetooth::audio::asrc::SourceAudioHalAsrc>(
	asrc_clock_source, /channels/ 2,		/channels/ 2,
	/sample_rate/ codec_in_use == CODEC_G722_24KHZ ? 24000 : 16000,		/sample_rate/ codec_in_use == CODEC_G722_24KHZ ? 24000 : 16000,
	/bit_depth/ 16,		/bit_depth/ 16,
	/interval_us/ default_data_interval_ms * 1000,		/interval_us/ default_data_interval_ms * 1000,
	/num_burst_buffers/ 0,		/num_burst_buffers/ 0,
	/burst_delay/ 0);		/burst_delay/ 0);
	}		}

	for (auto& device : hearingDevices.devices) {
	if (!device.accepting_audio) {
	continue;
	}

	uint16_t lcid = GAP_ConnGetL2CAPCid(device.gap_handle);
	uint16_t rcid = 0;
	L2CA_GetRemoteCid(lcid, &rcid);

	auto conn = btm_acl_for_bda(device.address, BT_TRANSPORT_LE);
	log::info("Updating ASRC context for handle=0x{:x}, cid=0x{:x}",
	conn->Handle(), rcid);

	asrc_clock_source->Update(device.isLeft(), conn->Handle(), rcid);
	}
	}		}

	// Reset the ASHA resampling context.		// Reset the ASHA resampling context.
	void ResetAsrc() {		void ResetAsrc() {
	log::info("Resetting the Asha resampling context");		log::info("Resetting the Asha resampling context");
	asrc_clock_source = nullptr;
	asrc = nullptr;		asrc = nullptr;
	}		}