Add balance/fade/treble/bass support

Changes:
- Add support for new VSS signals for balance/fade/treble/bass.
  Treble and bass drive the recently added equalizer controls,
  while balance and fade currently do simple linear scaling down
  of the non-emphasized direction.
- Add support for VSS 4.0 separate navigation volume and mute
  signals.
- Significant refactoring of the audiomixer API calling code in
  the AudioMixerService class to avoid code duplication.
- All pushes of the updated VSS values for volume and mute
  signals now are driven from the mixer API callback so that
  internal and external mixer changes are handled with less
  code duplication.  For now, gain changes have been kept as
  explicit VSS updates on set, this may change if handling
  external changes seems more worthwhile.  As well, initial
  master volume setting is a bit of a special case, see below.
- Setting the initial volume has been tweaked a bit to ensure
  that the value gets pushed out via VSS.  There currently seems
  to be an issue with the master playback sink mixer not taking
  changes until playback starts, so while the current code
  ensures that VSS subscribers see the value the daemon has set,
  it is likely that does not correspond to what's in WirePlumber
  until playback of some form happens and another volume change
  is made.  This will be addressed with upstream.
- Change flags in the mixer API have been updated to indicate
  overall and per-channel volume changes as separate flags.

Bug-AGL: SPEC-5001

Change-Id: I66585f573978989a0c281f060a667b4495e4bc0f
Signed-off-by: Scott Murray <scott.murray@konsulko.com>

3 files changed, 310 insertions, 73 deletions

diff --git a/src/AudiomixerService.cpp b/src/AudiomixerService.cpp
index f2445b8..5a3861f 100644
--- a/src/AudiomixerService.cpp
+++ b/src/AudiomixerService.cpp
@@ -13,6 +13,20 @@
 
 #include "AudiomixerService.h"
 
+static const std::string VSS_VOLUME = "Vehicle.Cabin.Infotainment.Media.Volume";
+static const std::string VSS_NAV_VOLUME = "Vehicle.Cabin.Infotainment.Navigation.Volume";
+static const std::string VSS_NAV_MUTE = "Vehicle.Cabin.Infotainment.Navigation.Mute";
+static const std::string VSS_SWITCH_VOL_UP = "Vehicle.Cabin.SteeringWheel.Switches.VolumeUp";
+static const std::string VSS_SWITCH_VOL_DOWN = "Vehicle.Cabin.SteeringWheel.Switches.VolumeDown";
+static const std::string VSS_SWITCH_VOL_MUTE = "Vehicle.Cabin.SteeringWheel.Switches.VolumeMute";
+static const std::string VSS_BALANCE = "Vehicle.Cabin.Infotainment.Media.Audio.Balance";
+static const std::string VSS_FADE = "Vehicle.Cabin.Infotainment.Media.Audio.Fade";
+static const std::string VSS_BASS = "Vehicle.Cabin.Infotainment.Media.Audio.Bass";
+static const std::string VSS_TREBLE = "Vehicle.Cabin.Infotainment.Media.Audio.Treble";
+
+static const char* PLAYBACK_MAIN = "Master Playback";
+static const char* PLAYBACK_NAV = "Playback: Navigation";
+
 AudiomixerService::AudiomixerService(const KuksaConfig &config, GMainLoop *loop) :
 	m_loop(loop),
 	m_config(config)
@@ -32,16 +46,6 @@ AudiomixerService::AudiomixerService(const KuksaConfig &config, GMainLoop *loop)
 		std::cerr << "Could not create WirePlumber connection" << std::endl;
 	}
 
-	// Set initial volume
-	if (m_audiomixer) {
-		audiomixer_lock(m_audiomixer);
-		const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, "Master Playback");
-		if (ctl) {
-			audiomixer_change_volume(m_audiomixer, ctl, 0.5);
-		}
-		audiomixer_unlock(m_audiomixer);
-	}
-
 	// Create gRPC channel
 	std::string host = m_config.hostname();
 	host += ":";
@@ -77,10 +81,18 @@ AudiomixerService::AudiomixerService(const KuksaConfig &config, GMainLoop *loop)
 		// Listen to actuator target updates for the volume signal and
 		// values for the steering wheel volume switch signals.
 		std::map<std::string, bool> signals;
-		signals["Vehicle.Cabin.Infotainment.Media.Volume"] = true;
-		signals["Vehicle.Cabin.SteeringWheel.Switches.VolumeUp"] = false;
-		signals["Vehicle.Cabin.SteeringWheel.Switches.VolumeDown"] = false;
-		signals["Vehicle.Cabin.SteeringWheel.Switches.VolumeMute"] = false;
+		signals[VSS_VOLUME] = true;
+		signals[VSS_NAV_VOLUME] = true;
+		signals[VSS_NAV_MUTE] = true;
+		// AGL additions
+		signals[VSS_SWITCH_VOL_UP] = false;
+		signals[VSS_SWITCH_VOL_DOWN] = false;
+		signals[VSS_SWITCH_VOL_MUTE] = false;
+		signals[VSS_BALANCE] = true;
+		signals[VSS_FADE] = true;
+		signals[VSS_BASS] = true;
+		signals[VSS_TREBLE] = true;
+
 		m_broker->subscribe(signals,
 				    [this](const std::string &path, const Datapoint &dp) {
 					    HandleSignalChange(path, dp);
@@ -89,15 +101,24 @@ AudiomixerService::AudiomixerService(const KuksaConfig &config, GMainLoop *loop)
 					    HandleSubscribeDone(request, s);
 				    });
 
-		// Set initial volume in VSS
-		// Push out the default value of 50 which matches the default in the
-		// homescreen app. Ideally there would be some form of persistence
-		// scheme to restore the last value on restart.
-		m_broker->set("Vehicle.Cabin.Infotainment.Media.Volume",
-			      50U,
-			      [this](const std::string &path, const Error &error) {
-				      HandleSignalSetError(path, error);
-			      });
+		// Set initial volume
+		// Push out default value of 50%, ideally there would be some
+		// form of persistence scheme to restore the last value on
+		// restart.
+		SetMainVolume();
+
+		// Push out new value
+		// NOTE: This is a workaround for current observed behavior
+		//       where no updates seem to get processed until playback
+		//       has happened.
+		uint32_t value = (m_volume + 0.005) * 100;
+		if (value <= 100) {
+			m_broker->set(VSS_VOLUME,
+				      value,
+				      [this](const std::string &path, const Error &error) {
+					      HandleSignalSetError(path, error);
+				      });
+		}
 	} else {
 		std::cerr << "Could not create Kuksa API client" << std::endl;
 	}
@@ -122,64 +143,65 @@ void AudiomixerService::HandleSignalChange(const std::string &path, const Datapo
 
 	audiomixer_lock(m_audiomixer);
 
-	const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, "Master Playback");
-	if (!ctl) {
-		audiomixer_unlock(m_audiomixer);
-		return;
-	}
-	if (path == "Vehicle.Cabin.Infotainment.Media.Volume") {
+	if (path == VSS_VOLUME) {
 		if (dp.has_uint32()) {
-			uint32_t volume = dp.uint32();
-			if (volume >= 0 && volume <= 100) {
-				double v = (double) volume / 100.0;
-				if (m_config.verbose() > 1)
-					std::cout << "Setting volume to " << v << std::endl;
-				audiomixer_change_volume(m_audiomixer, ctl, v);
+			double volume = (double) dp.uint32() / 100.0;
+			if (volume >= 0.0 && volume <= 1.0) {
+				m_volume = volume;
+				SetMainVolume();
+			}
+		}
+	} else if (path == VSS_NAV_VOLUME) {
+		if (dp.has_uint32()) {
+			double volume = (double) dp.uint32() / 100.0;
+			if (volume >= 0.0 && volume <= 1.0) {
+				m_nav_volume = volume;
+				SetMixerVolume(path, PLAYBACK_NAV, volume);
+			}
+		}
+	} else if (path == VSS_NAV_MUTE) {
+		if (dp.has_string()) {
+			std::string value = dp.string();
+			bool mute = false;
+			if (value.length()) {
+				if (value == "MUTED")
+					mute = true;
+				// else UNMUTED or ALERT_ONLY (which is not handled)
 
-				// Push out new value
-				m_broker->set("Vehicle.Cabin.Infotainment.Media.Volume",
-					      volume,
-					      [this](const std::string &path, const Error &error) {
-						      HandleSignalSetError(path, error);
-					      });
+				m_nav_mute = mute;
+				SetMixerMute(path, PLAYBACK_NAV, mute);
 			}
 		}
-	} else if (path == "Vehicle.Cabin.SteeringWheel.Switches.VolumeUp") {
+	} else if (path == VSS_SWITCH_VOL_UP) {
 		if (dp.has_bool_() && dp.bool_()) {
-			double volume = ctl->volume;
+			double volume = m_volume;
 			volume += 0.05; // up 5%
 			if (volume > 1.0)
 				volume = 1.0; // clamp to 100%
 			if (m_config.verbose() > 1)
 				std::cout << "Increasing volume to " << volume << std::endl;
-			audiomixer_change_volume(m_audiomixer, ctl, volume);
-
-			// Push out new value
-			m_broker->set("Vehicle.Cabin.Infotainment.Media.Volume",
-				      (unsigned) (volume * 100),
-				      [this](const std::string &path, const Error &error) {
-					      HandleSignalSetError(path, error);
-				      });
+			m_volume = volume;
+			SetMainVolume();
 		}
-	} else if (path == "Vehicle.Cabin.SteeringWheel.Switches.VolumeDown") {
+	} else if (path == VSS_SWITCH_VOL_DOWN) {
 		if (dp.has_bool_() && dp.bool_()) {
-			double volume = ctl->volume;
+			double volume = m_volume;
 			volume -= 0.05; // down 5%
 			if (volume < 0.0)
 				volume = 0.0; // clamp to 0%
 			if (m_config.verbose() > 1)
 				std::cout << "Decreasing volume to " << volume << std::endl;
-			audiomixer_change_volume(m_audiomixer, ctl, volume);
-
-			// Push out new value
-			m_broker->set("Vehicle.Cabin.Infotainment.Media.Volume",
-				      (unsigned) (volume * 100),
-				      [this](const std::string &path, const Error &error) {
-					      HandleSignalSetError(path, error);
-				      });
+			m_volume = volume;
+			SetMainVolume();
 		}
-	} else if (path == "Vehicle.Cabin.SteeringWheel.Switches.VolumeMute") {
+	} else if (path == VSS_SWITCH_VOL_MUTE) {
 		if (dp.has_bool_() && dp.bool_()) {
+			// No equivalent VSS signal, so call audiomixer API directly
+			const struct mixer_control *ctl =
+				audiomixer_find_control(m_audiomixer, PLAYBACK_MAIN);
+			if (!ctl)
+				goto control_error;
+
 			if (m_config.verbose() > 1) {
 				if (ctl->mute)
 					std::cout << "Unmuting" << std::endl;
@@ -188,9 +210,64 @@ void AudiomixerService::HandleSignalChange(const std::string &path, const Datapo
 			}
 			audiomixer_change_mute(m_audiomixer, ctl, !ctl->mute);
 		}
+	} else if (path == VSS_BALANCE) {
+		if (dp.has_int32()) {
+			int32_t value = dp.int32();
+			if (value >= -100 && value <= 100) {
+				if (m_config.verbose() > 1)
+					std::cout << "AudiomixerService::HandleSignalChange: Value =  "
+						  << value << std::endl;
+				m_balance = value;
+				SetMainVolume();
+				
+				// Push out new value
+				m_broker->set(VSS_BALANCE,
+					      value,
+					      [this](const std::string &path, const Error &error) {
+						      HandleSignalSetError(path, error);
+					      });
+			}
+		}
+	} else if (path == VSS_FADE) {
+		if (dp.has_int32()) {
+			int32_t value = dp.int32();
+			if (value >= -100 && value <= 100) {
+				if (m_config.verbose() > 1)
+					std::cout << "AudiomixerService::HandleSignalChange: Value =  "
+						  << value << std::endl;
+				m_fade = value;
+				SetMainVolume();
+
+				// Push out new value
+				m_broker->set(VSS_FADE,
+					      value,
+					      [this](const std::string &path, const Error &error) {
+						      HandleSignalSetError(path, error);
+					      });
+			}
+		}
+	} else if (path == VSS_BASS) {
+		if (dp.has_int32()) {
+			int32_t value = dp.int32();
+			if (m_config.verbose() > 1)
+				std::cout << "AudiomixerService::HandleSignalChange: Value =  "
+					  << value << std::endl;
+			float gain = value * 0.12;
+			SetEqualizerGain(path, "bass", gain);
+		}
+	} else if (path == VSS_TREBLE) {
+		if (dp.has_int32()) {
+			int32_t value = dp.int32();
+			if (m_config.verbose() > 1)
+				std::cout << "AudiomixerService::HandleSignalChange: Value =  "
+					  << value << std::endl;
+			float gain = value * 0.12;
+			SetEqualizerGain(path, "treble", gain);
+		}
 	}
 	// else ignore
 
+control_error:
 	audiomixer_unlock(m_audiomixer);
 }
 
@@ -202,8 +279,8 @@ void AudiomixerService::HandleSignalSetError(const std::string &path, const Erro
 void AudiomixerService::HandleSubscribeDone(const SubscribeRequest *request, const Status &status)
 {
 	if (m_config.verbose())
-		std::cout << "Subscribe status = " << status.error_code() <<
-			" (" << status.error_message() << ")" << std::endl;
+		std::cout << "Subscribe status = " << status.error_code()
+			  << " (" << status.error_message() << ")" << std::endl;
 
 	if (status.error_code() == grpc::CANCELLED) {
 		if (m_config.verbose())
@@ -263,16 +340,160 @@ void AudiomixerService::HandleMixerValueChange(unsigned int change_mask, const s
 		return;
 
 	if (change_mask & MIXER_CONTROL_CHANGE_FLAG_VOLUME) {
-		if (std::string(control->name) == "Master Playback") {
+		if (std::string(control->name) == PLAYBACK_MAIN) {
+			// Detecting changes is complicated by the reported volume
+			// always being the same as the left channel volume.
+			// With our balance logic, the greater of the two volumes
+			// should always equal what we consider the overall volume
+			// to be.  We do not handle calculating a new balance
+			// value from an external change.
+			double volume = control->lvolume;
+			if (control->rvolume > control->lvolume)
+				volume = control->rvolume;
+			if (fabs(m_volume_faded - volume) >= 0.00001) {
+				std::cout << "WARNING: external volume change seen" << std::endl;
+
+				// Attempt to rebase our state
+				m_volume_faded = volume;
+				if (m_fade < 0)
+					m_volume = volume / (1.0 - (m_fade / -100.0));
+				else
+					m_volume = volume;
+			}
+
+			// Push out new value
+			uint32_t value = (m_volume + 0.005) * 100;
+			if (value <= 100) {
+				m_broker->set(VSS_VOLUME,
+					      value,
+					      [this](const std::string &path, const Error &error) {
+						      HandleSignalSetError(path, error);
+					      });
+			}
+		} else if (std::string(control->name) == PLAYBACK_NAV) {
+			if (control->volume != m_nav_volume) {
+				std::cout << "WARNING: external Navigation volume change seen" << std::endl;
+				m_nav_volume = control->volume;
+			}
+
+			// Push out new value
+			uint32_t value = (control->volume + 0.005) * 100;
+			if (value <= 100) {
+				m_broker->set(VSS_NAV_VOLUME,
+					      value,
+					      [this](const std::string &path, const Error &error) {
+						      HandleSignalSetError(path, error);
+					      });
+			}
+		}
+	} else if (change_mask & MIXER_CONTROL_CHANGE_FLAG_MUTE) {
+		if (std::string(control->name) == PLAYBACK_NAV) {
+			if (control->mute != m_nav_mute) {
+				std::cout << "WARNING: external Navigation mute change seen" << std::endl;
+				m_nav_mute = control->mute;
+			}
+
 			// Push out new value
-			unsigned value = control->volume * 100;
-			m_broker->set("Vehicle.Cabin.Infotainment.Media.Volume",
+			const char *value = "UNMUTED";
+			if (control->mute)
+				value = "MUTED";
+			m_broker->set(VSS_NAV_MUTE,
 				      value,
 				      [this](const std::string &path, const Error &error) {
 					      HandleSignalSetError(path, error);
 				      });
 		}
-	} else if (change_mask & MIXER_CONTROL_CHANGE_FLAG_MUTE) {
-		// For now, do nothing, new state is in control->mute
+		// Else, do nothing, new state is in control->mute
+	}
+	// External gain control changes ignored for now
+}
+
+// NOTE: These functions assume the audiomixer lock is held
+
+void AudiomixerService::SetMainVolume()
+{
+	const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, PLAYBACK_MAIN);
+	if (!ctl)
+		return;
+
+	if (m_balance == 0) {
+		if (m_config.verbose() > 1)
+			std::cout << "Setting 'Master Playback' volume to "
+				  << m_volume
+				  << std::endl;
+		// Handle fade
+		m_volume_faded = m_volume;
+		if (m_fade < 0)
+			m_volume_faded *= (1.0 - (m_fade / -100.0));
+
+		audiomixer_change_volume(m_audiomixer, ctl, m_volume_faded);
+	} else {
+		// Handle fade
+		m_volume_faded = m_volume;
+		if (m_fade < 0)
+			m_volume_faded *= (1.0 - (m_fade / -100.0));
+
+		// Scale down the non-emphasized channel
+		double lvol = m_volume_faded;
+		double rvol = m_volume_faded;
+		if (m_balance < 0)
+			rvol = (1.0 - (m_balance / -100.0)) * m_volume_faded;
+		else if (m_balance > 0)
+			lvol = (1.0 - (m_balance / 100.0)) * m_volume_faded;
+
+		if (m_config.verbose() > 1)
+			std::cout << "Setting 'Master Playback' volume to ("
+				  << lvol
+				  << ", "
+				  << rvol
+				  << ")"
+				  << std::endl;
+		audiomixer_change_channel_volume(m_audiomixer, ctl, lvol, rvol);
 	}
 }
+
+void AudiomixerService::SetMixerVolume(const std::string &path, const std::string &mixer, const double volume)
+{
+	if (volume < 0.0 || volume > 1.0)
+		return;
+
+	const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, mixer.c_str());
+	if (!ctl)
+		return;
+
+	if (m_config.verbose() > 1)
+		std::cout << "Setting '" << ctl->name << "' volume to " << volume << std::endl;
+	audiomixer_change_volume(m_audiomixer, ctl, volume);
+}
+
+void AudiomixerService::SetMixerMute(const std::string path, const std::string mixer, const bool mute)
+{
+	const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, mixer.c_str());
+	if (!ctl)
+		return;
+
+	if (m_config.verbose() > 1)
+		std::cout << "Setting '" << mixer << "' mute to " << mute << std::endl;
+	audiomixer_change_mute(m_audiomixer, ctl, mute);
+}
+
+void AudiomixerService::SetEqualizerGain(const std::string path, const std::string mixer, const double gain)
+{
+	if (gain < -12.0 || gain > 12.0)
+		return;
+
+	const struct mixer_control *ctl = audiomixer_find_control(m_audiomixer, mixer.c_str());
+	if (!ctl)
+		return;
+
+	if (m_config.verbose() > 1)
+		std::cout << "Setting '" << mixer << "' gain to " << gain << std::endl;
+	audiomixer_change_gain(m_audiomixer, ctl, gain);
+
+	// Push out new value
+	m_broker->set(path,
+		      (int32_t) (gain / 0.12),
+		      [this](const std::string &path, const Error &error) {
+			      HandleSignalSetError(path, error);
+		      });
+}
diff --git a/src/AudiomixerService.h b/src/AudiomixerService.h
index 3cd87c6..dd26743 100644
--- a/src/AudiomixerService.h
+++ b/src/AudiomixerService.h
@@ -56,6 +56,13 @@ private:
 	struct audiomixer *m_audiomixer;
 	struct audiomixer_events m_audiomixer_events;
 
+	double m_volume = 0.5;
+	double m_volume_faded = 0.5;
+	int m_balance = 0;
+	int m_fade = 0;
+	double m_nav_volume = 0.5;
+	bool m_nav_mute = false;
+
 	void HandleSignalChange(const std::string &path, const Datapoint &dp);
 
 	void HandleSignalSetError(const std::string &path, const Error &error);
@@ -67,6 +74,14 @@ private:
 	void HandleMixerControlChange(void);
 
 	void HandleMixerValueChange(unsigned int change_mask, const struct mixer_control *control);
+
+	void SetMainVolume(void);
+
+	void SetMixerVolume(const std::string &path, const std::string &mixer, const double volume);
+
+	void SetMixerMute(const std::string path, const std::string mixer, const bool mute);
+
+	void SetEqualizerGain(const std::string path, const std::string mixer, const double gain);
 };
 
 #endif // _AUDIOMIXER_SERVICE_H
diff --git a/src/audiomixer.h b/src/audiomixer.h
index 1f089d1..941179f 100644
--- a/src/audiomixer.h
+++ b/src/audiomixer.h
@@ -26,15 +26,16 @@ struct mixer_control
 	bool mute;
 };
 
+#define MIXER_CONTROL_CHANGE_FLAG_VOLUME		(1<<0)
+#define MIXER_CONTROL_CHANGE_FLAG_CHANNEL_VOLUME	(1<<1)
+#define MIXER_CONTROL_CHANGE_FLAG_MUTE			(1<<2)
+#define MIXER_CONTROL_CHANGE_FLAG_GAIN			(1<<3)
+
 struct audiomixer_events
 {
 	void (*controls_changed) (void *data);
 
 	void (*value_changed) (void *data,
-#define MIXER_CONTROL_CHANGE_FLAG_VOLUME (1<<0)
-#define MIXER_CONTROL_CHANGE_FLAG_CHANNEL_VOLUME (1<<0)
-#define MIXER_CONTROL_CHANGE_FLAG_MUTE   (1<<1)
-#define MIXER_CONTROL_CHANGE_FLAG_GAIN (1<<2)
 				unsigned int change_mask,
 				const struct mixer_control *control);
 };