ipa: rpi: Add new algorithms for PiSP

Add new CAC, HDR, Saturation and Tonemapping algorithms.

Add a new Denoise algorithm that handles spatial/temporal/colour denoise
through one interface. With this change, the old SDN algorithm is now
considered deprecated and a warning message will be displayed if it is
enabled.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
Signed-off-by: Kieran Bingham <kieran.bingham@ideasonboard.com>

20 files changed, 987 insertions, 0 deletions

diff --git a/src/ipa/rpi/controller/agc_status.h b/src/ipa/rpi/controller/agc_status.h
index e5c4ee22..68f89958 100644
--- a/src/ipa/rpi/controller/agc_status.h
+++ b/src/ipa/rpi/controller/agc_status.h
@@ -10,6 +10,8 @@
 
 #include <libcamera/base/utils.h>
 
+#include "hdr_status.h"
+
 /*
  * The AGC algorithm process method should post an AgcStatus into the image
  * metadata under the tag "agc.status".
@@ -37,6 +39,7 @@ struct AgcStatus {
 	libcamera::utils::Duration fixedShutter;
 	double fixedAnalogueGain;
 	unsigned int channel;
+	HdrStatus hdr;
 };
 
 struct AgcPrepareStatus {
diff --git a/src/ipa/rpi/controller/cac_status.h b/src/ipa/rpi/controller/cac_status.h
new file mode 100644
index 00000000..475d4c5c
--- /dev/null
+++ b/src/ipa/rpi/controller/cac_status.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023 Raspberry Pi Ltd
+ *
+ * CAC (Chromatic Abberation Correction) algorithm status
+ */
+#pragma once
+
+#include "pwl.h"
+
+struct CacStatus {
+	std::vector<double> lutRx;
+	std::vector<double> lutRy;
+	std::vector<double> lutBx;
+	std::vector<double> lutBy;
+};
diff --git a/src/ipa/rpi/controller/denoise_status.h b/src/ipa/rpi/controller/denoise_status.h
index f6b9ee29..4d2bd291 100644
--- a/src/ipa/rpi/controller/denoise_status.h
+++ b/src/ipa/rpi/controller/denoise_status.h
@@ -14,3 +14,22 @@ struct DenoiseStatus {
 	double strength;
 	unsigned int mode;
 };
+
+struct SdnStatus {
+	double noiseConstant;
+	double noiseSlope;
+	double noiseConstant2;
+	double noiseSlope2;
+	double strength;
+};
+
+struct CdnStatus {
+	double strength;
+	double threshold;
+};
+
+struct TdnStatus {
+	double noiseConstant;
+	double noiseSlope;
+	double threshold;
+};
diff --git a/src/ipa/rpi/controller/hdr_algorithm.h b/src/ipa/rpi/controller/hdr_algorithm.h
new file mode 100644
index 00000000..f622e099
--- /dev/null
+++ b/src/ipa/rpi/controller/hdr_algorithm.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023, Raspberry Pi Ltd
+ *
+ * hdr_algorithm.h - HDR control algorithm interface
+ */
+#pragma once
+
+#include <vector>
+
+#include "algorithm.h"
+
+namespace RPiController {
+
+class HdrAlgorithm : public Algorithm
+{
+public:
+	HdrAlgorithm(Controller *controller)
+		: Algorithm(controller) {}
+	/* An HDR algorithm must provide the following: */
+	virtual int setMode(std::string const &modeName) = 0;
+	virtual std::vector<unsigned int> getChannels() const = 0;
+};
+
+} /* namespace RPiController */
diff --git a/src/ipa/rpi/controller/hdr_status.h b/src/ipa/rpi/controller/hdr_status.h
new file mode 100644
index 00000000..24b1a935
--- /dev/null
+++ b/src/ipa/rpi/controller/hdr_status.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023 Raspberry Pi Ltd
+ *
+ * hdr_status.h - HDR control algorithm status
+ */
+#pragma once
+
+#include <string>
+
+/*
+ * The HDR algorithm process method should post an HdrStatus into the image
+ * metadata under the tag "hdr.status".
+ */
+
+struct HdrStatus {
+	std::string mode;
+	std::string channel;
+};
diff --git a/src/ipa/rpi/controller/meson.build b/src/ipa/rpi/controller/meson.build
index 20b9cda9..32a4d31c 100644
--- a/src/ipa/rpi/controller/meson.build
+++ b/src/ipa/rpi/controller/meson.build
@@ -12,14 +12,19 @@ rpi_ipa_controller_sources = files([
     'rpi/alsc.cpp',
     'rpi/awb.cpp',
     'rpi/black_level.cpp',
+    'rpi/cac.cpp',
     'rpi/ccm.cpp',
     'rpi/contrast.cpp',
+    'rpi/denoise.cpp',
     'rpi/dpc.cpp',
     'rpi/geq.cpp',
+    'rpi/hdr.cpp',
     'rpi/lux.cpp',
     'rpi/noise.cpp',
+    'rpi/saturation.cpp',
     'rpi/sdn.cpp',
     'rpi/sharpen.cpp',
+    'rpi/tonemap.cpp',
 ])
 
 rpi_ipa_controller_deps = [
diff --git a/src/ipa/rpi/controller/rpi/cac.cpp b/src/ipa/rpi/controller/rpi/cac.cpp
new file mode 100644
index 00000000..7c123da1
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/cac.cpp
@@ -0,0 +1,81 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023 Raspberry Pi Ltd
+ *
+ * cac.cpp - Chromatic Aberration Correction algorithm
+ */
+#include "cac.h"
+
+#include <libcamera/base/log.h>
+
+#include "cac_status.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiCac)
+
+#define NAME "rpi.cac"
+
+Cac::Cac(Controller *controller)
+	: Algorithm(controller)
+{
+}
+
+char const *Cac::name() const
+{
+	return NAME;
+}
+
+int Cac::read(const libcamera::YamlObject &params)
+{
+	arrayToSet(params["lut_rx"], config_.lutRx);
+	arrayToSet(params["lut_ry"], config_.lutRy);
+	arrayToSet(params["lut_bx"], config_.lutBx);
+	arrayToSet(params["lut_by"], config_.lutBy);
+	cacStatus_.lutRx = config_.lutRx;
+	cacStatus_.lutRy = config_.lutRy;
+	cacStatus_.lutBx = config_.lutBx;
+	cacStatus_.lutBy = config_.lutBy;
+	double strength = params["strength"].get<double>(1);
+	setStrength(config_.lutRx, cacStatus_.lutRx, strength);
+	setStrength(config_.lutBx, cacStatus_.lutBx, strength);
+	setStrength(config_.lutRy, cacStatus_.lutRy, strength);
+	setStrength(config_.lutBy, cacStatus_.lutBy, strength);
+	return 0;
+}
+
+void Cac::initialise()
+{
+}
+
+void Cac::arrayToSet(const libcamera::YamlObject &params, std::vector<double> &inputArray)
+{
+	int num = 0;
+	const Size &size = getHardwareConfig().cacRegions;
+	inputArray.resize((size.width + 1) * (size.height + 1));
+	for (const auto &p : params.asList()) {
+		inputArray[num++] = p.get<double>(0);
+	}
+}
+
+void Cac::setStrength(std::vector<double> &inputArray, std::vector<double> &outputArray,
+		      double strengthFactor)
+{
+	int num = 0;
+	for (const auto &p : inputArray) {
+		outputArray[num++] = p * strengthFactor;
+	}
+}
+
+void Cac::prepare(Metadata *imageMetadata)
+{
+	imageMetadata->set("cac.status", cacStatus_);
+}
+
+// Register algorithm with the system.
+static Algorithm *Create(Controller *controller)
+{
+	return (Algorithm *)new Cac(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/rpi/controller/rpi/cac.h b/src/ipa/rpi/controller/rpi/cac.h
new file mode 100644
index 00000000..419180ab
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/cac.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023, Raspberry Pi Ltd
+ *
+ * cac.hpp - CAC control algorithm
+ */
+#pragma once
+
+#include "algorithm.h"
+#include "cac_status.h"
+
+namespace RPiController {
+
+struct CacConfig {
+	std::vector<double> lutRx;
+	std::vector<double> lutRy;
+	std::vector<double> lutBx;
+	std::vector<double> lutBy;
+};
+
+class Cac : public Algorithm
+{
+public:
+	Cac(Controller *controller = NULL);
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+	void prepare(Metadata *imageMetadata) override;
+	void setStrength(std::vector<double> &inputArray, std::vector<double> &outputArray,
+			 double strengthFactor);
+
+private:
+	CacConfig config_;
+	CacStatus cacStatus_;
+	void arrayToSet(const libcamera::YamlObject &params, std::vector<double> &inputArray);
+};
+
+} // namespace RPiController
diff --git a/src/ipa/rpi/controller/rpi/denoise.cpp b/src/ipa/rpi/controller/rpi/denoise.cpp
new file mode 100644
index 00000000..440ee442
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/denoise.cpp
@@ -0,0 +1,156 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Raspberry Pi Ltd
+ *
+ * Denoise.cpp - Denoise (spatial, colour, temporal) control algorithm
+ */
+#include "denoise.h"
+
+#include <libcamera/base/log.h>
+
+#include "denoise_status.h"
+#include "noise_status.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiDenoise)
+
+// Calculate settings for the denoise blocks using the noise profile in
+// the image metadata.
+
+#define NAME "rpi.denoise"
+
+Denoise::Denoise(Controller *controller)
+	: DenoiseAlgorithm(controller), mode_(DenoiseMode::ColourHighQuality)
+{
+}
+
+char const *Denoise::name() const
+{
+	return NAME;
+}
+
+int Denoise::read(const libcamera::YamlObject &params)
+{
+	sdnEnable_ = params.contains("sdn");
+	if (sdnEnable_) {
+		auto &sdnParams = params["sdn"];
+		sdnDeviation_ = sdnParams["deviation"].get<double>(3.2);
+		sdnStrength_ = sdnParams["strength"].get<double>(0.25);
+		sdnDeviation2_ = sdnParams["deviation2"].get<double>(sdnDeviation_);
+		sdnDeviationNoTdn_ = sdnParams["deviation_no_tdn"].get<double>(sdnDeviation_);
+		sdnStrengthNoTdn_ = sdnParams["strength_no_tdn"].get<double>(sdnStrength_);
+		sdnTdnBackoff_ = sdnParams["backoff"].get<double>(0.75);
+	}
+
+	cdnEnable_ = params.contains("cdn");
+	if (cdnEnable_) {
+		auto &cdnParams = params["cdn"];
+		cdnDeviation_ = cdnParams["deviation"].get<double>(120);
+		cdnStrength_ = cdnParams["strength"].get<double>(0.2);
+	}
+
+	tdnEnable_ = params.contains("tdn");
+	if (tdnEnable_) {
+		auto &tdnParams = params["tdn"];
+		tdnDeviation_ = tdnParams["deviation"].get<double>(0.5);
+		tdnThreshold_ = tdnParams["threshold"].get<double>(0.75);
+	} else if (sdnEnable_) {
+		/*
+		 * If SDN is enabled but TDN isn't, overwrite all the SDN settings
+		 * with the "no TDN" versions. This makes it easier to enable or
+		 * disable TDN in the tuning file without editing all the other
+		 * parameters.
+		 */
+		sdnDeviation_ = sdnDeviation2_ = sdnDeviationNoTdn_;
+		sdnStrength_ = sdnStrengthNoTdn_;
+	}
+
+	return 0;
+}
+
+void Denoise::initialise()
+{
+}
+
+void Denoise::switchMode([[maybe_unused]] CameraMode const &cameraMode,
+			 [[maybe_unused]] Metadata *metadata)
+{
+	/* A mode switch effectively resets temporal denoise and it has to start over. */
+	currentSdnDeviation_ = sdnDeviationNoTdn_;
+	currentSdnStrength_ = sdnStrengthNoTdn_;
+	currentSdnDeviation2_ = sdnDeviationNoTdn_;
+}
+
+void Denoise::prepare(Metadata *imageMetadata)
+{
+	struct NoiseStatus noiseStatus = {};
+	noiseStatus.noiseSlope = 3.0; // in case no metadata
+	if (imageMetadata->get("noise.status", noiseStatus) != 0)
+		LOG(RPiDenoise, Warning) << "no noise profile found";
+
+	LOG(RPiDenoise, Debug)
+		<< "Noise profile: constant " << noiseStatus.noiseConstant
+		<< " slope " << noiseStatus.noiseSlope;
+
+	if (mode_ == DenoiseMode::Off)
+		return;
+
+	if (sdnEnable_) {
+		struct SdnStatus sdn;
+		sdn.noiseConstant = noiseStatus.noiseConstant * currentSdnDeviation_;
+		sdn.noiseSlope = noiseStatus.noiseSlope * currentSdnDeviation_;
+		sdn.noiseConstant2 = noiseStatus.noiseConstant * sdnDeviation2_;
+		sdn.noiseSlope2 = noiseStatus.noiseSlope * currentSdnDeviation2_;
+		sdn.strength = currentSdnStrength_;
+		imageMetadata->set("sdn.status", sdn);
+		LOG(RPiDenoise, Debug)
+			<< "const " << sdn.noiseConstant
+			<< " slope " << sdn.noiseSlope
+			<< " str " << sdn.strength
+			<< " const2 " << sdn.noiseConstant2
+			<< " slope2 " << sdn.noiseSlope2;
+
+		/* For the next frame, we back off the SDN parameters as TDN ramps up. */
+		double f = sdnTdnBackoff_;
+		currentSdnDeviation_ = f * currentSdnDeviation_ + (1 - f) * sdnDeviation_;
+		currentSdnStrength_ = f * currentSdnStrength_ + (1 - f) * sdnStrength_;
+		currentSdnDeviation2_ = f * currentSdnDeviation2_ + (1 - f) * sdnDeviation2_;
+	}
+
+	if (tdnEnable_) {
+		struct TdnStatus tdn;
+		tdn.noiseConstant = noiseStatus.noiseConstant * tdnDeviation_;
+		tdn.noiseSlope = noiseStatus.noiseSlope * tdnDeviation_;
+		tdn.threshold = tdnThreshold_;
+		imageMetadata->set("tdn.status", tdn);
+		LOG(RPiDenoise, Debug)
+			<< "programmed tdn threshold " << tdn.threshold
+			<< " constant " << tdn.noiseConstant
+			<< " slope " << tdn.noiseSlope;
+	}
+
+	if (cdnEnable_ && mode_ != DenoiseMode::ColourOff) {
+		struct CdnStatus cdn;
+		cdn.threshold = cdnDeviation_ * noiseStatus.noiseSlope + noiseStatus.noiseConstant;
+		cdn.strength = cdnStrength_;
+		imageMetadata->set("cdn.status", cdn);
+		LOG(RPiDenoise, Debug)
+			<< "programmed cdn threshold " << cdn.threshold
+			<< " strength " << cdn.strength;
+	}
+}
+
+void Denoise::setMode(DenoiseMode mode)
+{
+	// We only distinguish between off and all other modes.
+	mode_ = mode;
+}
+
+// Register algorithm with the system.
+static Algorithm *Create(Controller *controller)
+{
+	return (Algorithm *)new Denoise(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/rpi/controller/rpi/denoise.h b/src/ipa/rpi/controller/rpi/denoise.h
new file mode 100644
index 00000000..88b37663
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/denoise.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi Ltd
+ *
+ * denoise.hpp - Denoise (spatial, colour, temporal) control algorithm
+ */
+#pragma once
+
+#include "algorithm.h"
+#include "denoise_algorithm.h"
+
+namespace RPiController {
+
+// Algorithm to calculate correct denoise settings.
+
+class Denoise : public DenoiseAlgorithm
+{
+public:
+	Denoise(Controller *controller);
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+	void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
+	void prepare(Metadata *imageMetadata) override;
+	void setMode(DenoiseMode mode) override;
+
+private:
+	double sdnDeviation_;
+	double sdnStrength_;
+	double sdnDeviation2_;
+	double sdnDeviationNoTdn_;
+	double sdnStrengthNoTdn_;
+	double sdnTdnBackoff_;
+	double cdnDeviation_;
+	double cdnStrength_;
+	double tdnDeviation_;
+	double tdnThreshold_;
+	DenoiseMode mode_;
+	bool tdnEnable_;
+	bool sdnEnable_;
+	bool cdnEnable_;
+
+	/* SDN parameters attenuate over time if TDN is running. */
+	double currentSdnDeviation_;
+	double currentSdnStrength_;
+	double currentSdnDeviation2_;
+};
+
+} // namespace RPiController
diff --git a/src/ipa/rpi/controller/rpi/hdr.cpp b/src/ipa/rpi/controller/rpi/hdr.cpp
new file mode 100644
index 00000000..295e4c5f
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/hdr.cpp
@@ -0,0 +1,270 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023 Raspberry Pi Ltd
+ *
+ * hdr.cpp - HDR control algorithm
+ */
+
+#include "hdr.h"
+
+#include <libcamera/base/log.h>
+
+#include "../agc_status.h"
+#include "../stitch_status.h"
+#include "../tonemap_status.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiHdr)
+
+#define NAME "rpi.hdr"
+
+void HdrConfig::read(const libcamera::YamlObject &params, const std::string &modeName)
+{
+	name = modeName;
+
+	if (!params.contains("cadence"))
+		LOG(RPiHdr, Fatal) << "No cadence for HDR mode " << name;
+	cadence = params["cadence"].getList<unsigned int>().value();
+	if (cadence.empty())
+		LOG(RPiHdr, Fatal) << "Empty cadence in HDR mode " << name;
+
+	/*
+	 * In the JSON file it's easier to use the channel name as the key, but
+	 * for us it's convenient to swap them over.
+	 */
+	for (const auto &[k, v] : params["channel_map"].asDict())
+		channelMap[v.get<unsigned int>().value()] = k;
+
+	/* Read any tonemap parameters. */
+	tonemapEnable = params["tonemap_enable"].get<int>(0);
+	detailConstant = params["detail_constant"].get<uint16_t>(50);
+	detailSlope = params["detail_slope"].get<double>(8.0);
+	iirStrength = params["iir_strength"].get<double>(8.0);
+	strength = params["strength"].get<double>(1.5);
+
+	if (tonemapEnable) {
+		/* We need either an explicit tonemap, or the information to build them dynamically. */
+		if (params.contains("tonemap")) {
+			if (tonemap.read(params["tonemap"]))
+				LOG(RPiHdr, Fatal) << "Failed to read tonemap in HDR mode " << name;
+		} else {
+			if (target.read(params["target"]))
+				LOG(RPiHdr, Fatal) << "Failed to read target in HDR mode " << name;
+			if (maxSlope.read(params["max_slope"]))
+				LOG(RPiHdr, Fatal) << "Failed to read max_slope in HDR mode " << name;
+			minSlope = params["min_slope"].get<double>(1.0);
+			maxGain = params["max_gain"].get<double>(64.0);
+			step = params["step"].get<double>(0.05);
+			speed = params["speed"].get<double>(0.5);
+		}
+	}
+
+	/* Read any stitch parameters. */
+	stitchEnable = params["stitch_enable"].get<int>(0);
+	thresholdLo = params["threshold_lo"].get<uint16_t>(50000);
+	motionThreshold = params["motion_threshold"].get<double>(0.005);
+	diffPower = params["diff_power"].get<uint8_t>(13);
+	if (diffPower > 15)
+		LOG(RPiHdr, Fatal) << "Bad diff_power value in HDR mode " << name;
+}
+
+Hdr::Hdr(Controller *controller)
+	: HdrAlgorithm(controller)
+{
+}
+
+char const *Hdr::name() const
+{
+	return NAME;
+}
+
+int Hdr::read(const libcamera::YamlObject &params)
+{
+	/* Make an "HDR off" mode by default so that tuning files don't have to. */
+	HdrConfig &offMode = config_["Off"];
+	offMode.name = "Off";
+	offMode.cadence = { 0 };
+	offMode.channelMap[0] = "None";
+	status_.mode = offMode.name;
+	delayedStatus_.mode = offMode.name;
+
+	/*
+	 * But we still allow the tuning file to override the "Off" mode if it wants.
+	 * For example, maybe an application will make channel 0 be the "short"
+	 * channel, in order to apply other AGC controls to it.
+	 */
+	for (const auto &[key, value] : params.asDict())
+		config_[key].read(value, key);
+
+	return 0;
+}
+
+int Hdr::setMode(std::string const &mode)
+{
+	/* Always validate the mode, so it can be used later without checking. */
+	auto it = config_.find(mode);
+	if (it == config_.end()) {
+		LOG(RPiHdr, Warning) << "No such HDR mode " << mode;
+		return -1;
+	}
+
+	status_.mode = it->second.name;
+
+	return 0;
+}
+
+std::vector<unsigned int> Hdr::getChannels() const
+{
+	return config_.at(status_.mode).cadence;
+}
+
+void Hdr::updateAgcStatus(Metadata *metadata)
+{
+	std::scoped_lock lock(*metadata);
+	AgcStatus *agcStatus = metadata->getLocked<AgcStatus>("agc.status");
+	if (agcStatus) {
+		HdrConfig &hdrConfig = config_[status_.mode];
+		auto it = hdrConfig.channelMap.find(agcStatus->channel);
+		if (it != hdrConfig.channelMap.end()) {
+			status_.channel = it->second;
+			agcStatus->hdr = status_;
+		} else
+			LOG(RPiHdr, Warning) << "Channel " << agcStatus->channel
+					     << " not found in mode " << status_.mode;
+	} else
+		LOG(RPiHdr, Warning) << "No agc.status found";
+}
+
+void Hdr::switchMode([[maybe_unused]] CameraMode const &cameraMode, Metadata *metadata)
+{
+	updateAgcStatus(metadata);
+	delayedStatus_ = status_;
+}
+
+bool Hdr::updateTonemap(StatisticsPtr &stats, HdrConfig &config)
+{
+	/* When there's a change of HDR mode we start over with a new tonemap curve. */
+	if (delayedStatus_.mode != previousMode_) {
+		previousMode_ = delayedStatus_.mode;
+		tonemap_ = Pwl();
+	}
+
+	/* No tonemapping. No need to output a tonemap.status. */
+	if (!config.tonemapEnable)
+		return false;
+
+	/* If an explicit tonemap was given, use it. */
+	if (!config.tonemap.empty()) {
+		tonemap_ = config.tonemap;
+		return true;
+	}
+
+	/*
+	 * We only update the tonemap on short frames when in multi-exposure mode. But
+	 * we still need to output the most recent tonemap. Possibly we should make the
+	 * config indicate the channels for which we should update the tonemap?
+	 */
+	if (delayedStatus_.mode == "MultiExposure" && delayedStatus_.channel != "short")
+		return true;
+
+	/* Build the tonemap dynamically using the image histogram. */
+	Pwl tonemap;
+	tonemap.append(0, 0);
+
+	double prev_input_val = 0;
+	double prev_output_val = 0;
+	const double step2 = config.step / 2;
+	for (double q = config.step; q < 1.0 - step2; q += config.step) {
+		double q_lo = std::max(0.0, q - step2);
+		double q_hi = std::min(1.0, q + step2);
+		double iqm = stats->yHist.interQuantileMean(q_lo, q_hi);
+		double input_val = std::min(iqm * 64, 65535.0);
+
+		if (input_val > prev_input_val + 1) {
+			/* We're going to calcualte a Pwl to map input_val to this output_val. */
+			double want_output_val = config.target.eval(q) * 65535;
+			/* But we must ensure we aren't applying too small or too great a local gain. */
+			double want_slope = (want_output_val - prev_output_val) / (input_val - prev_input_val);
+			double slope = std::clamp(want_slope, config.minSlope,
+						  config.maxSlope.eval(q));
+			double output_val = prev_output_val + slope * (input_val - prev_input_val);
+			output_val = std::min(output_val, config.maxGain * input_val);
+			output_val = std::clamp(output_val, 0.0, 65535.0);
+			/* Let the tonemap adapte slightly more gently from frame to frame. */
+			if (!tonemap_.empty()) {
+				double old_output_val = tonemap_.eval(input_val);
+				output_val = config.speed * output_val +
+					     (1 - config.speed) * old_output_val;
+			}
+			LOG(RPiHdr, Debug) << "q " << q << " input " << input_val
+					   << " output " << want_output_val << " slope " << want_slope
+					   << " slope " << slope << " output " << output_val;
+			tonemap.append(input_val, output_val);
+			prev_input_val = input_val;
+			prev_output_val = output_val;
+		}
+	}
+
+	tonemap.append(65535, 65535);
+	/* tonemap.debug(); */
+	tonemap_ = tonemap;
+
+	return true;
+}
+
+void Hdr::process(StatisticsPtr &stats, Metadata *imageMetadata)
+{
+	/* Note what HDR channel this frame will be once it comes back to us. */
+	updateAgcStatus(imageMetadata);
+
+	/*
+	 * Now figure out what HDR channel this frame is. It should be available in the
+	 * agc.delayed_status, unless this is an early frame after a mode switch, in which
+	 * case delayedStatus_ should be right.
+	 */
+	AgcStatus agcStatus;
+	if (!imageMetadata->get<AgcStatus>("agc.delayed_status", agcStatus))
+		delayedStatus_ = agcStatus.hdr;
+
+	auto it = config_.find(delayedStatus_.mode);
+	if (it == config_.end()) {
+		/* Shouldn't be possible. There would be nothing we could do. */
+		LOG(RPiHdr, Warning) << "Unexpected HDR mode " << delayedStatus_.mode;
+		return;
+	}
+
+	HdrConfig &config = it->second;
+
+	if (updateTonemap(stats, config)) {
+		/* Add tonemap.status metadata. */
+		TonemapStatus tonemapStatus;
+
+		tonemapStatus.detailConstant = config.detailConstant;
+		tonemapStatus.detailSlope = config.detailSlope;
+		tonemapStatus.iirStrength = config.iirStrength;
+		tonemapStatus.strength = config.strength;
+		tonemapStatus.tonemap = tonemap_;
+
+		imageMetadata->set("tonemap.status", tonemapStatus);
+	}
+
+	if (config.stitchEnable) {
+		/* Add stitch.status metadata. */
+		StitchStatus stitchStatus;
+
+		stitchStatus.diffPower = config.diffPower;
+		stitchStatus.motionThreshold = config.motionThreshold;
+		stitchStatus.thresholdLo = config.thresholdLo;
+
+		imageMetadata->set("stitch.status", stitchStatus);
+	}
+}
+
+/* Register algorithm with the system. */
+static Algorithm *create(Controller *controller)
+{
+	return (Algorithm *)new Hdr(controller);
+}
+static RegisterAlgorithm reg(NAME, &create);
diff --git a/src/ipa/rpi/controller/rpi/hdr.h b/src/ipa/rpi/controller/rpi/hdr.h
new file mode 100644
index 00000000..01ba45f1
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/hdr.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023, Raspberry Pi Ltd
+ *
+ * hdr.h - HDR control algorithm
+ */
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "../hdr_algorithm.h"
+#include "../hdr_status.h"
+#include "../pwl.h"
+
+/* This is our implementation of an HDR algorithm. */
+
+namespace RPiController {
+
+struct HdrConfig {
+	std::string name;
+	std::vector<unsigned int> cadence;
+	std::map<unsigned int, std::string> channelMap;
+
+	/* Tonemap related parameters. */
+	bool tonemapEnable;
+	uint16_t detailConstant;
+	double detailSlope;
+	double iirStrength;
+	double strength;
+	/* We must have either an explicit tonemap curve, or the other parameters. */
+	Pwl tonemap;
+	Pwl target; /* maps histogram quatile to desired target output value */
+	Pwl maxSlope; /* the maximum slope allowed at each point in the mapping */
+	double minSlope; /* the minimum allowed slope */
+	double maxGain; /* limit to the max absolute gain */
+	double step; /* the histogram granularity for building the mapping */
+	double speed; /* rate at which tonemap is updated */
+
+	/* Stitch related parameters. */
+	bool stitchEnable;
+	uint16_t thresholdLo;
+	uint8_t diffPower;
+	double motionThreshold;
+
+	void read(const libcamera::YamlObject &params, const std::string &name);
+};
+
+class Hdr : public HdrAlgorithm
+{
+public:
+	Hdr(Controller *controller);
+	char const *name() const override;
+	void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
+	int read(const libcamera::YamlObject &params) override;
+	void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
+	int setMode(std::string const &mode) override;
+	std::vector<unsigned int> getChannels() const override;
+
+private:
+	void updateAgcStatus(Metadata *metadata);
+	bool updateTonemap(StatisticsPtr &stats, HdrConfig &config);
+
+	std::map<std::string, HdrConfig> config_;
+	HdrStatus status_; /* track the current HDR mode and channel */
+	HdrStatus delayedStatus_; /* track the delayed HDR mode and channel */
+	std::string previousMode_;
+	Pwl tonemap_;
+};
+
+} /* namespace RPiController */
diff --git a/src/ipa/rpi/controller/rpi/saturation.cpp b/src/ipa/rpi/controller/rpi/saturation.cpp
new file mode 100644
index 00000000..813540e5
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/saturation.cpp
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Raspberry Pi Ltd
+ *
+ * saturation.cpp - Saturation control algorithm
+ */
+#include "saturation.h"
+
+#include <libcamera/base/log.h>
+
+#include "saturation_status.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiSaturation)
+
+#define NAME "rpi.saturation"
+
+Saturation::Saturation(Controller *controller)
+	: Algorithm(controller)
+{
+}
+
+char const *Saturation::name() const
+{
+	return NAME;
+}
+
+int Saturation::read(const libcamera::YamlObject &params)
+{
+	config_.shiftR = params["shift_r"].get<uint8_t>(0);
+	config_.shiftG = params["shift_g"].get<uint8_t>(0);
+	config_.shiftB = params["shift_b"].get<uint8_t>(0);
+	return 0;
+}
+
+void Saturation::initialise()
+{
+}
+
+void Saturation::prepare(Metadata *imageMetadata)
+{
+	SaturationStatus saturation;
+
+	saturation.shiftR = config_.shiftR;
+	saturation.shiftG = config_.shiftG;
+	saturation.shiftB = config_.shiftB;
+	imageMetadata->set("saturation.status", saturation);
+}
+
+// Register algorithm with the system.
+static Algorithm *Create(Controller *controller)
+{
+	return (Algorithm *)new Saturation(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/rpi/controller/rpi/saturation.h b/src/ipa/rpi/controller/rpi/saturation.h
new file mode 100644
index 00000000..97da412a
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/saturation.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi Ltd
+ *
+ * saturation.hpp - Saturation control algorithm
+ */
+#pragma once
+
+#include "algorithm.h"
+
+namespace RPiController {
+
+struct SaturationConfig {
+	uint8_t shiftR;
+	uint8_t shiftG;
+	uint8_t shiftB;
+};
+
+class Saturation : public Algorithm
+{
+public:
+	Saturation(Controller *controller = NULL);
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+	void prepare(Metadata *imageMetadata) override;
+
+private:
+	SaturationConfig config_;
+};
+
+} // namespace RPiController
diff --git a/src/ipa/rpi/controller/rpi/sdn.cpp b/src/ipa/rpi/controller/rpi/sdn.cpp
index b6b66251..6743919e 100644
--- a/src/ipa/rpi/controller/rpi/sdn.cpp
+++ b/src/ipa/rpi/controller/rpi/sdn.cpp
@@ -36,6 +36,8 @@ char const *Sdn::name() const
 
 int Sdn::read(const libcamera::YamlObject &params)
 {
+	LOG(RPiSdn, Warning)
+		<< "Using legacy SDN tuning - please consider moving SDN inside rpi.denoise";
 	deviation_ = params["deviation"].get<double>(3.2);
 	strength_ = params["strength"].get<double>(0.75);
 	return 0;
diff --git a/src/ipa/rpi/controller/rpi/tonemap.cpp b/src/ipa/rpi/controller/rpi/tonemap.cpp
new file mode 100644
index 00000000..5f8b2bf2
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/tonemap.cpp
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Raspberry Pi Ltd
+ *
+ * tonemap.cpp - Tonemap control algorithm
+ */
+#include "tonemap.h"
+
+#include <libcamera/base/log.h>
+
+#include "tonemap_status.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiTonemap)
+
+#define NAME "rpi.tonemap"
+
+Tonemap::Tonemap(Controller *controller)
+	: Algorithm(controller)
+{
+}
+
+char const *Tonemap::name() const
+{
+	return NAME;
+}
+
+int Tonemap::read(const libcamera::YamlObject &params)
+{
+	config_.detailConstant = params["detail_constant"].get<uint16_t>(0);
+	config_.detailSlope = params["detail_slope"].get<double>(0.1);
+	config_.iirStrength = params["iir_strength"].get<double>(1.0);
+	config_.strength = params["strength"].get<double>(1.0);
+	config_.tonemap.read(params["tone_curve"]);
+	return 0;
+}
+
+void Tonemap::initialise()
+{
+}
+
+void Tonemap::prepare(Metadata *imageMetadata)
+{
+	TonemapStatus tonemapStatus;
+
+	tonemapStatus.detailConstant = config_.detailConstant;
+	tonemapStatus.detailSlope = config_.detailSlope;
+	tonemapStatus.iirStrength = config_.iirStrength;
+	tonemapStatus.strength = config_.strength;
+	tonemapStatus.tonemap = config_.tonemap;
+	imageMetadata->set("tonemap.status", tonemapStatus);
+}
+
+// Register algorithm with the system.
+static Algorithm *Create(Controller *controller)
+{
+	return (Algorithm *)new Tonemap(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/rpi/controller/rpi/tonemap.h b/src/ipa/rpi/controller/rpi/tonemap.h
new file mode 100644
index 00000000..f25aa47f
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/tonemap.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi Ltd
+ *
+ * tonemap.hpp - Tonemap control algorithm
+ */
+#pragma once
+
+#include "algorithm.h"
+#include "pwl.h"
+
+namespace RPiController {
+
+struct TonemapConfig {
+	uint16_t detailConstant;
+	double detailSlope;
+	double iirStrength;
+	double strength;
+	Pwl tonemap;
+};
+
+class Tonemap : public Algorithm
+{
+public:
+	Tonemap(Controller *controller = NULL);
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+	void prepare(Metadata *imageMetadata) override;
+
+private:
+	TonemapConfig config_;
+};
+
+} // namespace RPiController
diff --git a/src/ipa/rpi/controller/saturation_status.h b/src/ipa/rpi/controller/saturation_status.h
new file mode 100644
index 00000000..337b66a3
--- /dev/null
+++ b/src/ipa/rpi/controller/saturation_status.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Raspberry Pi Ltd
+ *
+ * saturation_status.h - Saturation control algorithm status
+ */
+#pragma once
+
+struct SaturationStatus {
+	uint8_t shiftR;
+	uint8_t shiftG;
+	uint8_t shiftB;
+};
diff --git a/src/ipa/rpi/controller/stitch_status.h b/src/ipa/rpi/controller/stitch_status.h
new file mode 100644
index 00000000..b17800ed
--- /dev/null
+++ b/src/ipa/rpi/controller/stitch_status.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2023 Raspberry Pi Ltd
+ *
+ * stitch_status.h - stitch control algorithm status
+ */
+#pragma once
+
+/*
+ * Parameters for the stitch block.
+ */
+
+struct StitchStatus {
+	uint16_t thresholdLo;
+	uint8_t diffPower;
+	double motionThreshold;
+};
diff --git a/src/ipa/rpi/controller/tonemap_status.h b/src/ipa/rpi/controller/tonemap_status.h
new file mode 100644
index 00000000..0e639946
--- /dev/null
+++ b/src/ipa/rpi/controller/tonemap_status.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Raspberry Pi Ltd
+ *
+ * hdr.h - Tonemap control algorithm status
+ */
+#pragma once
+
+#include "pwl.h"
+
+struct TonemapStatus {
+	uint16_t detailConstant;
+	double detailSlope;
+	double iirStrength;
+	double strength;
+	RPiController::Pwl tonemap;
+};