ipa: rpi: agc: Implementation of multi-channel AGC

The switchMode, prepare and process methods are updated to implement
multi-channel AGC correctly:

* switchMode now invokes switchMode on all the channels (whether
  active or not).

* prepare must find what channel the current frame is, and run on
  behalf of that channel.

* process updates the most recent DeviceStatus and statistics for the
  channel of the frame that has just arrived, but generates updated
  values working through the active channels in round-robin fashion.

One minor detail in process is that we don't want to change the
DeviceStatus metadata of the current frame, so we now pass this to the
AgcChannel's process method, rather than letting it find the
DeviceStatus in the metadata.

Signed-off-by: David Plowman <david.plowman@raspberrypi.com>
Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: Jacopo Mondi <jacopo.mondi@ideasonboard.com>
Signed-off-by: Jacopo Mondi <jacopo.mondi@ideasonboard.com>

5 files changed, 116 insertions, 22 deletions

diff --git a/src/ipa/rpi/controller/agc_status.h b/src/ipa/rpi/controller/agc_status.h
index 597eddd7..e5c4ee22 100644
--- a/src/ipa/rpi/controller/agc_status.h
+++ b/src/ipa/rpi/controller/agc_status.h
@@ -36,6 +36,7 @@ struct AgcStatus {
 	int floatingRegionEnable;
 	libcamera::utils::Duration fixedShutter;
 	double fixedAnalogueGain;
+	unsigned int channel;
 };
 
 struct AgcPrepareStatus {
diff --git a/src/ipa/rpi/controller/rpi/agc.cpp b/src/ipa/rpi/controller/rpi/agc.cpp
index 598fc890..8b9d2026 100644
--- a/src/ipa/rpi/controller/rpi/agc.cpp
+++ b/src/ipa/rpi/controller/rpi/agc.cpp
@@ -22,7 +22,7 @@ LOG_DEFINE_CATEGORY(RPiAgc)
 
 Agc::Agc(Controller *controller)
 	: AgcAlgorithm(controller),
-	  activeChannels_({ 0 })
+	  activeChannels_({ 0 }), index_(0)
 {
 }
 
@@ -205,20 +205,116 @@ void Agc::setActiveChannels(const std::vector<unsigned int> &activeChannels)
 void Agc::switchMode(CameraMode const &cameraMode,
 		     Metadata *metadata)
 {
-	LOG(RPiAgc, Debug) << "switchMode for channel 0";
-	channelData_[0].channel.switchMode(cameraMode, metadata);
+	/*
+	 * We run switchMode on every channel, and then we're going to start over
+	 * with the first active channel again which means that this channel's
+	 * status needs to be the one we leave in the metadata.
+	 */
+	AgcStatus status;
+
+	for (unsigned int channelIndex = 0; channelIndex < channelData_.size(); channelIndex++) {
+		LOG(RPiAgc, Debug) << "switchMode for channel " << channelIndex;
+		channelData_[channelIndex].channel.switchMode(cameraMode, metadata);
+		if (channelIndex == activeChannels_[0])
+			metadata->get("agc.status", status);
+	}
+
+	status.channel = activeChannels_[0];
+	metadata->set("agc.status", status);
+	index_ = 0;
+}
+
+static void getDelayedChannelIndex(Metadata *metadata, const char *message, unsigned int &channelIndex)
+{
+	std::unique_lock<RPiController::Metadata> lock(*metadata);
+	AgcStatus *status = metadata->getLocked<AgcStatus>("agc.delayed_status");
+	if (status)
+		channelIndex = status->channel;
+	else {
+		/* This does happen at startup, otherwise it would be a Warning or Error. */
+		LOG(RPiAgc, Debug) << message;
+	}
+}
+
+static void setCurrentChannelIndex(Metadata *metadata, const char *message, unsigned int channelIndex)
+{
+	std::unique_lock<RPiController::Metadata> lock(*metadata);
+	AgcStatus *status = metadata->getLocked<AgcStatus>("agc.status");
+	if (status)
+		status->channel = channelIndex;
+	else {
+		/* This does happen at startup, otherwise it would be a Warning or Error. */
+		LOG(RPiAgc, Debug) << message;
+	}
 }
 
 void Agc::prepare(Metadata *imageMetadata)
 {
-	LOG(RPiAgc, Debug) << "prepare for channel 0";
-	channelData_[0].channel.prepare(imageMetadata);
+	/*
+	 * The DeviceStatus in the metadata should be correct for the image we
+	 * are processing. The delayed status should tell us what channel this frame
+	 * was from, so we will use that channel's prepare method.
+	 *
+	 * \todo To be honest, there's not much that's stateful in the prepare methods
+	 * so we should perhaps re-evaluate whether prepare even needs to be done
+	 * "per channel".
+	 */
+	unsigned int channelIndex = activeChannels_[0];
+	getDelayedChannelIndex(imageMetadata, "prepare: no delayed status", channelIndex);
+
+	LOG(RPiAgc, Debug) << "prepare for channel " << channelIndex;
+	channelData_[channelIndex].channel.prepare(imageMetadata);
 }
 
 void Agc::process(StatisticsPtr &stats, Metadata *imageMetadata)
 {
-	LOG(RPiAgc, Debug) << "process for channel 0";
-	channelData_[0].channel.process(stats, imageMetadata);
+	/*
+	 * We want to generate values for the next channel in round robin fashion
+	 * (i.e. the channel at location index_ in the activeChannel list), even though
+	 * the statistics we have will be for a different channel (which we find
+	 * again from the delayed status).
+	 */
+
+	/* Generate updated AGC values for channel for new channel that we are requesting. */
+	unsigned int channelIndex = activeChannels_[index_];
+	AgcChannelData &channelData = channelData_[channelIndex];
+	/* The stats that arrived with this image correspond to the following channel. */
+	unsigned int statsIndex = 0;
+	getDelayedChannelIndex(imageMetadata, "process: no delayed status for stats", statsIndex);
+	LOG(RPiAgc, Debug) << "process for channel " << channelIndex;
+
+	/*
+	 * We keep a cache of the most recent DeviceStatus and stats for each channel,
+	 * so that we can invoke the next channel's process method with the most up to date
+	 * values.
+	 */
+	LOG(RPiAgc, Debug) << "Save DeviceStatus and stats for channel " << statsIndex;
+	DeviceStatus deviceStatus;
+	if (imageMetadata->get<DeviceStatus>("device.status", deviceStatus) == 0)
+		channelData_[statsIndex].deviceStatus = deviceStatus;
+	else
+		/* Every frame should have a DeviceStatus. */
+		LOG(RPiAgc, Error) << "process: no device status found";
+	channelData_[statsIndex].statistics = stats;
+
+	/*
+	 * Finally fetch the most recent DeviceStatus and stats for the new channel, if both
+	 * exist, and call process(). We must make the agc.status metadata record correctly
+	 * which channel this is.
+	 */
+	if (channelData.statistics && channelData.deviceStatus) {
+		deviceStatus = *channelData.deviceStatus;
+		stats = channelData.statistics;
+	} else {
+		/* Can also happen when new channels start. */
+		LOG(RPiAgc, Debug) << "process: channel " << channelIndex << " not seen yet";
+	}
+
+	channelData.channel.process(stats, deviceStatus, imageMetadata);
+	setCurrentChannelIndex(imageMetadata, "process: no AGC status found", channelIndex);
+
+	/* And onto the next channel for the next call. */
+	index_ = (index_ + 1) % activeChannels_.size();
 }
 
 /* Register algorithm with the system. */
diff --git a/src/ipa/rpi/controller/rpi/agc.h b/src/ipa/rpi/controller/rpi/agc.h
index a476ea70..ee85c693 100644
--- a/src/ipa/rpi/controller/rpi/agc.h
+++ b/src/ipa/rpi/controller/rpi/agc.h
@@ -6,6 +6,7 @@
  */
 #pragma once
 
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -17,6 +18,8 @@ namespace RPiController {
 
 struct AgcChannelData {
 	AgcChannel channel;
+	std::optional<DeviceStatus> deviceStatus;
+	StatisticsPtr statistics;
 };
 
 class Agc : public AgcAlgorithm
@@ -51,6 +54,7 @@ private:
 	int checkChannel(unsigned int channel) const;
 	std::vector<AgcChannelData> channelData_;
 	std::vector<unsigned int> activeChannels_;
+	unsigned int index_; /* index into the activeChannels_ */
 };
 
 } /* namespace RPiController */
diff --git a/src/ipa/rpi/controller/rpi/agc_channel.cpp b/src/ipa/rpi/controller/rpi/agc_channel.cpp
index f180b848..84eafe7d 100644
--- a/src/ipa/rpi/controller/rpi/agc_channel.cpp
+++ b/src/ipa/rpi/controller/rpi/agc_channel.cpp
@@ -444,7 +444,7 @@ void AgcChannel::prepare(Metadata *imageMetadata)
 	}
 }
 
-void AgcChannel::process(StatisticsPtr &stats, Metadata *imageMetadata)
+void AgcChannel::process(StatisticsPtr &stats, DeviceStatus const &deviceStatus, Metadata *imageMetadata)
 {
 	frameCount_++;
 	/*
@@ -455,7 +455,7 @@ void AgcChannel::process(StatisticsPtr &stats, Metadata *imageMetadata)
 	/* Fetch the AWB status immediately, so that we can assume it's there. */
 	fetchAwbStatus(imageMetadata);
 	/* Get the current exposure values for the frame that's just arrived. */
-	fetchCurrentExposure(imageMetadata);
+	fetchCurrentExposure(deviceStatus);
 	/* Compute the total gain we require relative to the current exposure. */
 	double gain, targetY;
 	computeGain(stats, imageMetadata, gain, targetY);
@@ -567,18 +567,11 @@ void AgcChannel::housekeepConfig()
 			   << meteringModeName_;
 }
 
-void AgcChannel::fetchCurrentExposure(Metadata *imageMetadata)
+void AgcChannel::fetchCurrentExposure(DeviceStatus const &deviceStatus)
 {
-	std::unique_lock<Metadata> lock(*imageMetadata);
-	DeviceStatus *deviceStatus =
-		imageMetadata->getLocked<DeviceStatus>("device.status");
-	if (!deviceStatus)
-		LOG(RPiAgc, Fatal) << "No device metadata";
-	current_.shutter = deviceStatus->shutterSpeed;
-	current_.analogueGain = deviceStatus->analogueGain;
-	AgcStatus *agcStatus =
-		imageMetadata->getLocked<AgcStatus>("agc.status");
-	current_.totalExposure = agcStatus ? agcStatus->totalExposureValue : 0s;
+	current_.shutter = deviceStatus.shutterSpeed;
+	current_.analogueGain = deviceStatus.analogueGain;
+	current_.totalExposure = 0s; /* this value is unused */
 	current_.totalExposureNoDG = current_.shutter * current_.analogueGain;
 }
 
diff --git a/src/ipa/rpi/controller/rpi/agc_channel.h b/src/ipa/rpi/controller/rpi/agc_channel.h
index aca79ef2..b2f554db 100644
--- a/src/ipa/rpi/controller/rpi/agc_channel.h
+++ b/src/ipa/rpi/controller/rpi/agc_channel.h
@@ -85,13 +85,13 @@ public:
 	void disableAuto();
 	void switchMode(CameraMode const &cameraMode, Metadata *metadata);
 	void prepare(Metadata *imageMetadata);
-	void process(StatisticsPtr &stats, Metadata *imageMetadata);
+	void process(StatisticsPtr &stats, DeviceStatus const &deviceStatus, Metadata *imageMetadata);
 
 private:
 	bool updateLockStatus(DeviceStatus const &deviceStatus);
 	AgcConfig config_;
 	void housekeepConfig();
-	void fetchCurrentExposure(Metadata *imageMetadata);
+	void fetchCurrentExposure(DeviceStatus const &deviceStatus);
 	void fetchAwbStatus(Metadata *imageMetadata);
 	void computeGain(StatisticsPtr &statistics, Metadata *imageMetadata,
 			 double &gain, double &targetY);