From abb300750f65c362eb100891f71a418c01b07d01 Mon Sep 17 00:00:00 2001
From: Arun Raghavan <arun@asymptotic.io>
Date: Thu, 8 Jun 2023 17:37:04 -0400
Subject: [PATCH] alsa: Implement playback/capture rate control for USB gadgets

If we detect Playback/Capture Pitch 1000000, we can adjust those values
to update the feedback endpoint for the host. On the capture side, this
will instruct the host to adjust the rate at which data is being sent.
On the playback side, this will adjust the amount of data the USB gadget
driver sends out in each USB tick.
---
 spa/plugins/alsa/alsa-pcm-sink.c   |  1 +
 spa/plugins/alsa/alsa-pcm-source.c |  1 +
 spa/plugins/alsa/alsa-pcm.c        | 99 +++++++++++++++++++++++++++++-
 spa/plugins/alsa/alsa-pcm.h        |  6 ++
 4 files changed, 105 insertions(+), 2 deletions(-)

diff --git a/spa/plugins/alsa/alsa-pcm-sink.c b/spa/plugins/alsa/alsa-pcm-sink.c
index 249d5db4a..b1256251c 100644
--- a/spa/plugins/alsa/alsa-pcm-sink.c
+++ b/spa/plugins/alsa/alsa-pcm-sink.c
@@ -771,6 +771,7 @@ impl_node_port_set_io(void *object,
 		break;
 	case SPA_IO_RateMatch:
 		this->rate_match = data;
+		spa_alsa_update_rate_match(this);
 		break;
 	default:
 		return -ENOENT;
diff --git a/spa/plugins/alsa/alsa-pcm-source.c b/spa/plugins/alsa/alsa-pcm-source.c
index 2867c901a..cccbd12ab 100644
--- a/spa/plugins/alsa/alsa-pcm-source.c
+++ b/spa/plugins/alsa/alsa-pcm-source.c
@@ -709,6 +709,7 @@ impl_node_port_set_io(void *object,
 		break;
 	case SPA_IO_RateMatch:
 		this->rate_match = data;
+		spa_alsa_update_rate_match(this);
 		break;
 	default:
 		return -ENOENT;
diff --git a/spa/plugins/alsa/alsa-pcm.c b/spa/plugins/alsa/alsa-pcm.c
index 52a68c292..fbf453aa3 100644
--- a/spa/plugins/alsa/alsa-pcm.c
+++ b/spa/plugins/alsa/alsa-pcm.c
@@ -546,6 +546,53 @@ int spa_alsa_clear(struct state *state)
 	return err;
 }
 
+static int probe_pitch_ctl(struct state *state, const char* device_name)
+{
+	snd_ctl_elem_id_t *id;
+	/* TODO: Add configuration params for the control name and units */
+	const char *elem_name =
+		state->stream == SND_PCM_STREAM_CAPTURE ?
+		"Capture Pitch 1000000" :
+		"Playback Pitch 1000000";
+	int err;
+
+	err = snd_ctl_open(&state->ctl, device_name, SND_CTL_NONBLOCK);
+
+	if (err < 0) {
+		spa_log_info(state->log, "%s could not find ctl device", state->props.device);
+		state->ctl = NULL;
+		return err;
+	}
+
+	snd_ctl_elem_id_alloca(&id);
+	snd_ctl_elem_id_set_name(id, elem_name);
+	snd_ctl_elem_id_set_interface(id, SND_CTL_ELEM_IFACE_PCM);
+
+	snd_ctl_elem_value_malloc(&state->pitch_elem);
+	snd_ctl_elem_value_set_id(state->pitch_elem, id);
+
+	err = snd_ctl_elem_read(state->ctl, state->pitch_elem);
+
+	if (err < 0) {
+		spa_log_debug(state->log, "%s: did not find ctl %s", state->props.device, elem_name);
+
+		snd_ctl_elem_value_free(state->pitch_elem);
+		state->pitch_elem = NULL;
+
+		snd_ctl_close(state->ctl);
+		state->ctl = NULL;
+
+		return err;
+	}
+
+	snd_ctl_elem_value_set_integer(state->pitch_elem, 0, 1000000);
+	state->last_rate = 1.0;
+
+	spa_log_info(state->log, "%s: found ctl %s", state->props.device, elem_name);
+
+	return 0;
+}
+
 int spa_alsa_open(struct state *state, const char *params)
 {
 	int err;
@@ -588,6 +635,8 @@ int spa_alsa_open(struct state *state, const char *params)
 	state->sample_count = 0;
 	state->sample_time = 0;
 
+	probe_pitch_ctl(state, device_name);
+
 	return 0;
 
 error_exit_close:
@@ -622,6 +671,14 @@ int spa_alsa_close(struct state *state)
 	state->have_format = false;
 	state->opened = false;
 
+	if (state->pitch_elem) {
+		snd_ctl_elem_value_free(state->pitch_elem);
+		state->pitch_elem = NULL;
+
+		snd_ctl_close(state->ctl);
+		state->ctl = NULL;
+	}
+
 	return err;
 }
 
@@ -1665,6 +1722,41 @@ int spa_alsa_set_format(struct state *state, struct spa_audio_info *fmt, uint32_
 	return match ? 0 : 1;
 }
 
+int spa_alsa_update_rate_match(struct state *state)
+{
+	uint64_t pitch, last_pitch;
+	int err;
+
+	if (!state->pitch_elem)
+		return -ENOENT;
+
+	/* The rate/pitch defines the rate of input to output (if there were a
+	 * resampler, it's the ratio of input samples to output samples). This
+	 * means that to adjust the playback rate, we need to apply the inverse
+	 * of the given rate. */
+	if (state->stream == SND_PCM_STREAM_CAPTURE) {
+		pitch = 1000000 * state->rate_match->rate;
+		last_pitch = 1000000 * state->last_rate;
+	} else {
+		pitch = 1000000 / state->rate_match->rate;
+		last_pitch = 1000000 / state->last_rate;
+	}
+
+	/* The pitch adjustment is limited to 1 ppm */
+	if (pitch == last_pitch)
+		return 0;
+
+	snd_ctl_elem_value_set_integer(state->pitch_elem, 0, pitch);
+	CHECK(snd_ctl_elem_write(state->ctl, state->pitch_elem), "snd_ctl_elem_write");
+
+	spa_log_trace_fp(state->log, "%s %u set rate to %g",
+			state->props.device, state->stream, state->rate_match->rate);
+
+	state->last_rate = state->rate_match->rate;
+
+	return 0;
+}
+
 static int set_swparams(struct state *state)
 {
 	snd_pcm_t *hndl = state->hndl;
@@ -2015,7 +2107,10 @@ static int update_time(struct state *state, uint64_t current_time, snd_pcm_sfram
 		else
 			state->rate_match->rate = 1.0/corr;
 
-		SPA_FLAG_UPDATE(state->rate_match->flags, SPA_IO_RATE_MATCH_FLAG_ACTIVE, state->matching);
+		if (state->pitch_elem && state->matching)
+			spa_alsa_update_rate_match(state);
+		else
+			SPA_FLAG_UPDATE(state->rate_match->flags, SPA_IO_RATE_MATCH_FLAG_ACTIVE, state->matching);
 	}
 
 	state->next_time += state->threshold / corr * 1e9 / state->rate;
@@ -2055,7 +2150,7 @@ static int setup_matching(struct state *state)
 	if (spa_streq(state->position->clock.name, state->clock_name))
 		state->matching = false;
 
-	state->resample = ((uint32_t)state->rate != state->rate_denom) || state->matching;
+	state->resample = !state->pitch_elem && (((uint32_t)state->rate != state->rate_denom) || state->matching);
 
 	spa_log_info(state->log, "driver clock:'%s'@%d our clock:'%s'@%d matching:%d resample:%d",
 			state->position->clock.name, state->rate_denom,
diff --git a/spa/plugins/alsa/alsa-pcm.h b/spa/plugins/alsa/alsa-pcm.h
index a7755bf05..a1295c6ec 100644
--- a/spa/plugins/alsa/alsa-pcm.h
+++ b/spa/plugins/alsa/alsa-pcm.h
@@ -218,6 +218,11 @@ struct state {
 
 	struct spa_latency_info latency[2];
 	struct spa_process_latency_info process_latency;
+
+	/* Rate match via an ALSA ctl */
+	snd_ctl_t *ctl;
+	snd_ctl_elem_value_t *pitch_elem;
+	double last_rate;
 };
 
 struct spa_pod *spa_alsa_enum_propinfo(struct state *state,
@@ -230,6 +235,7 @@ int spa_alsa_enum_format(struct state *state, int seq,
 		     const struct spa_pod *filter);
 
 int spa_alsa_set_format(struct state *state, struct spa_audio_info *info, uint32_t flags);
+int spa_alsa_update_rate_match(struct state *state);
 
 int spa_alsa_init(struct state *state, const struct spa_dict *info);
 int spa_alsa_clear(struct state *state);