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	Diff line number	Diff line
		@@ -14,10 +14,26 @@ cc_library_static {
		"asrc/asrc_resampler.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: [
		"libchrome",
		],
		static_libs: [
		"libbase",
		"libbluetooth_hci_pdl",
		"libbluetooth_log",
		"libbt_shim_bridge",
		"libflatbuffers-cpp",
		],
		host_supported: true,
		@@ -31,17 +47,33 @@ cc_library_host_shared {
		name: "libasrc_resampler_test",
		defaults: ["bluetooth_cflags"],
		srcs: [
		":TestMockMainShimEntry",
		"asrc/asrc_resampler_test.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: [
		"libbluetooth_hci_pdl",
		"libbluetooth_log",
		"libbt-common",
		"libbt_shim_bridge",
		"libchrome",
		"libevent",
		"libflatbuffers-cpp",
		"libgmock",
		],
		stl: "libc++_static",
		include_dirs: [
		"packages/modules/Bluetooth/system",
		],
		generated_headers: [
		"BluetoothGeneratedDumpsysDataSchema_h",
		],

system/audio/asrc/asrc_resampler.cc

+73 −123

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

		#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 {

		class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
		const int interval_;

		class SourceAudioHalAsrc::ClockRecovery
		: public bluetooth::hal::ReadClockHandler {
		std::mutex mutex_;
		bluetooth::common::RepeatingTimer read_clock_timer_;

		unsigned num_produced_;

		enum class LinkState { RESET, WARMUP, RUNNING };
		enum class StateId { RESET, WARMUP, RUNNING };

		struct {
		struct {
		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;
		StateId id;

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

		uint32_t decim_t0;
		int decim_dt[2];

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

		__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& link = state.link[link_id];

		// Setup the start point of the streaming

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

		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.local_time = state.stream_time = state.t0;
		state.last_bt_clock = bt_clock;

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

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

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

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

		int dt_current = int(timestamp_us - link.local_time);
		link.decim_dt[1] = std::min(link.decim_dt[1], dt_current);
		uint32_t local_time = state.local_time + elapsed_us;
		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 deviation between local time and stream time in this interval can be
		// ignored.

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

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

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

		int drift = link.decim_dt[1] - link.decim_dt[0];
		link.decim_dt[0] = link.decim_dt[1];
		link.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;
		int drift = state.decim_dt[1] - state.decim_dt[0];
		state.decim_dt[0] = state.decim_dt[1];
		state.decim_dt[1] = INT_MAX;

		// Let's filter the derive, with a low-pass Butterworth filter.
		// 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
		// 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 +=
		(int(ldexpf(state.butter_drift, 8)) - err + (1 << 7)) >> 8;

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

		decltype(output_stats_) output_stats;
		int min_buffer_level;

		{
		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",
		output_stats.sample_rate,
		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;

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

		public:
		ClockRecovery(unsigned interval_us)
		: interval_(interval_us),
		num_produced_(0),
		state_{
		.link = {{.state = LinkState::RESET}, {.state = LinkState::RESET}},
		.active_link_id = -1},
		reference_timing_{0, 0, 0} {}
		ClockRecovery() : state_{.id = StateId::RESET}, reference_timing_{0, 0, 0} {
		read_clock_timer_.SchedulePeriodic(
		get_main_thread()->GetWeakPtr(), FROM_HERE,
		base::BindRepeating(
		[](void*) {
		bluetooth::shim::GetHciLayer()->EnqueueCommand(
		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(
		uint32_t stream_time) {
		@@ -254,13 +206,12 @@ class SourceAudioHalAsrc::ClockRecovery : public ClockHandler {
		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,
		// this should be used for logging.

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

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

		#endif

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

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

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

		// 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

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

		if (0)

system/audio/asrc/asrc_resampler.h

+3 −17

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

		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 {
		public:
		// 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
		// transmission intervals is done.

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

		~SourceAudioHalAsrc();

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

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

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

system/audio/asrc/asrc_resampler_test.cc

+11 −6

Original line number	Diff line number	Diff line
		@@ -19,17 +19,22 @@
		#include <cstdio>
		#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 {
		void Bind(ClockHandler*) override {}
		};
		namespace bluetooth::hal {
		void LinkClocker::Register(ReadClockHandler*) {}
		void LinkClocker::Unregister() {}
		} // namespace bluetooth::hal

		namespace bluetooth::audio::asrc {

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

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

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

		// Create a new ASRC context if required.
		if (asrc == nullptr) {
		asrc_clock_source =
		std::make_shared<bluetooth::hal::L2capCreditIndEvents>();
		log::info("Configuring Asha resampler");
		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,
		/bit_depth/ 16,
		/interval_us/ default_data_interval_ms * 1000,
		/num_burst_buffers/ 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.
		void ResetAsrc() {
		log::info("Resetting the Asha resampling context");
		asrc_clock_source = nullptr;
		asrc = nullptr;
		}