1 files changed, 290 insertions, 69 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);
+		      });
+}