[PATCH v5 09/20] iio: adc: ad7768: Configure channel sampling profiles

[Janani Sunil <janani.sunil@analog.com>]

At buffered capture setup, select the fastest compatible power mode for
the enabled channel rates. Group channels into the two hardware mode
profiles and program their decimation, mode selection, and data clock.

Signed-off-by: Janani Sunil <janani.sunil@analog.com>
---
 drivers/iio/adc/ad7768.c | 153 ++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 126 insertions(+), 27 deletions(-)

diff --git a/drivers/iio/adc/ad7768.c b/drivers/iio/adc/ad7768.c
index 09b4e2f32304..00d6f1dce591 100644
--- a/drivers/iio/adc/ad7768.c
+++ b/drivers/iio/adc/ad7768.c
@@ -7,7 +7,9 @@
 
 #include <linux/array_size.h>
 #include <linux/bitfield.h>
+#include <linux/bitmap.h>
 #include <linux/bitops.h>
+#include <linux/cleanup.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/device.h>
@@ -35,7 +37,6 @@
 
 #define AD7768_REG_CH_MODE(x)				(0x01 + (x))
 #define   AD7768_CH_MODE_FILTER_TYPE_MSK		BIT(3)
-#define     AD7768_CH_MODE_FILTER_TYPE_WIDEBAND		0x0
 #define   AD7768_CH_MODE_DEC_RATE_MSK			GENMASK(2, 0)
 
 #define AD7768_REG_CH_MODE_SEL				0x03
@@ -110,6 +111,7 @@
 #define AD7768_MAX_MCLK_FREQ_HZ				(34 * HZ_PER_MHZ)
 #define AD7768_MAX_FREQ_PER_MODE			6
 #define AD7768_MAX_CHANNEL				8
+#define AD7768_NUM_CHANNEL_MODES			2
 
 enum ad7768_clock_source {
 	AD7768_CLOCK_SOURCE_MCLK,
@@ -212,6 +214,11 @@ static u8 ad7768_channel_mask(const struct ad7768_state *st, u8 ch)
 	return BIT(st->chip_info->chan_map[ch]);
 }
 
+static u8 ad7768_channel_mode_mask(const struct ad7768_state *st, u8 ch)
+{
+	return BIT(ch) | ad7768_channel_mask(st, ch);
+}
+
 static u8 ad7768_all_standby_mask(const struct ad7768_state *st)
 {
 	return GENMASK(st->chip_info->num_channels - 1, 0);
@@ -460,14 +467,69 @@ static int ad7768_set_mode_decimation(struct ad7768_state *st,
 					     freq_cfg->dec_rate));
 }
 
-static int ad7768_update_scan_mode(struct iio_dev *indio_dev,
-				   const unsigned long *scan_mask)
+static int ad7768_set_lowest_noise_mode(struct ad7768_state *st,
+					const unsigned long *scan_mask)
+{
+	/*
+	 * The output data rate ranges overlap between the power modes. At a
+	 * common ODR, the faster mode has lower noise, so prefer the fastest
+	 * mode that supports every enabled channel.
+	 */
+	for (unsigned int mode_idx = ARRAY_SIZE(ad7768_power_modes);
+	     mode_idx-- > 0;) {
+		bool supported = true;
+		unsigned int channel;
+
+		for_each_set_bit(channel, scan_mask,
+				 st->chip_info->num_channels) {
+			if (!ad7768_find_freq_config(st, mode_idx,
+						     st->ch_freq[channel])) {
+				supported = false;
+				break;
+			}
+		}
+
+		if (supported)
+			return ad7768_set_power_mode(st, mode_idx);
+	}
+
+	return -EINVAL;
+}
+
+static int ad7768_find_matching_mode(const bool *mode_used,
+				     const unsigned int *mode_freq,
+				     unsigned int freq)
+{
+	for (unsigned int mode = 0; mode < AD7768_NUM_CHANNEL_MODES; mode++) {
+		if (!mode_used[mode] || mode_freq[mode] == freq)
+			return mode;
+	}
+
+	return -EINVAL;
+}
+
+static int ad7768_apply_channel_modes(struct iio_dev *indio_dev,
+				      const unsigned long *scan_mask)
 {
+	unsigned int mode_freq[AD7768_NUM_CHANNEL_MODES] = { };
+	bool mode_used[AD7768_NUM_CHANNEL_MODES] = { };
 	struct ad7768_state *st = iio_priv(indio_dev);
+	struct device *dev = indio_dev->dev.parent;
 	unsigned long channel_mask;
 	unsigned long standby_mask;
+	unsigned int max_freq = 0;
+	unsigned int c;
 	int ret;
 
+	guard(mutex)(&st->lock);
+
+	ret = ad7768_set_lowest_noise_mode(st, scan_mask);
+	if (ret == -EINVAL)
+		return dev_err_probe(dev, ret,
+				     "No power mode supports all ODRs\n");
+	if (ret)
+		return ret;
+
 	channel_mask = ad7768_all_standby_mask(st);
 	standby_mask = channel_mask & ~*scan_mask;
 
@@ -478,11 +540,65 @@ static int ad7768_update_scan_mode(struct iio_dev *indio_dev,
 	if (st->clock_source == AD7768_CLOCK_SOURCE_XTAL)
 		standby_mask &= ~BIT(st->chip_info->num_channels / 2);
 
+	for_each_set_bit(c, scan_mask, st->chip_info->num_channels) {
+		unsigned int mask;
+		int mode;
+
+		mode = ad7768_find_matching_mode(mode_used, mode_freq,
+						 st->ch_freq[c]);
+		if (mode < 0)
+			return dev_err_probe(dev, -EINVAL,
+					     "Over %d channel modes required\n",
+					     AD7768_NUM_CHANNEL_MODES);
+
+		mode_freq[mode] = st->ch_freq[c];
+		mode_used[mode] = true;
+
+		mask = ad7768_channel_mode_mask(st, c);
+		ret = regmap_assign_bits(st->regmap, AD7768_REG_CH_MODE_SEL,
+					 mask, mode);
+		if (ret)
+			return ret;
+	}
+
 	ret = regmap_update_bits(st->regmap, AD7768_REG_CH_STANDBY,
 				 channel_mask, standby_mask);
 	if (ret)
 		return ret;
 
+	for (unsigned int mode = 0; mode < AD7768_NUM_CHANNEL_MODES; mode++) {
+		if (!mode_used[mode])
+			continue;
+
+		ret = ad7768_set_mode_decimation(st, mode_freq[mode], mode);
+		if (ret)
+			return ret;
+
+		ret = regmap_clear_bits(st->regmap, AD7768_REG_CH_MODE(mode),
+					AD7768_CH_MODE_FILTER_TYPE_MSK);
+		if (ret)
+			return ret;
+
+		max_freq = max(max_freq, mode_freq[mode]);
+	}
+
+	ret = ad7768_set_clk_divs(st, max_freq);
+	if (ret)
+		return ret;
+
+	return ad7768_sync(st);
+}
+
+static int ad7768_update_scan_mode(struct iio_dev *indio_dev,
+				   const unsigned long *scan_mask)
+{
+	struct ad7768_state *st = iio_priv(indio_dev);
+	int ret;
+
+	ret = ad7768_apply_channel_modes(indio_dev, scan_mask);
+	if (ret)
+		return ret;
+
 	for (unsigned int c = 0; c < st->chip_info->num_channels; c++) {
 		if (test_bit(c, scan_mask))
 			ret = iio_backend_chan_enable(st->back, c);
@@ -629,11 +745,10 @@ static void ad7768_set_available_sampling_freqs(struct ad7768_state *st)
 		st->avail_freq[ARRAY_SIZE(ad7768_power_modes) - 1].n_freqs--;
 }
 
-static int ad7768_configure_capture(struct ad7768_state *st)
+static int ad7768_init_sampling_freqs(struct ad7768_state *st)
 {
 	const struct ad7768_avail_freq *avail_freq;
 	unsigned int default_freq;
-	unsigned int mode_config;
 	int ret;
 
 	ret = ad7768_set_power_mode(st, ARRAY_SIZE(ad7768_power_modes) - 1);
@@ -646,27 +761,7 @@ static int ad7768_configure_capture(struct ad7768_state *st)
 	     channel < st->chip_info->num_channels; channel++)
 		st->ch_freq[channel] = default_freq;
 
-	ret = ad7768_set_mode_decimation(st, default_freq, 0);
-	if (ret)
-		return ret;
-
-	mode_config = FIELD_PREP(AD7768_CH_MODE_FILTER_TYPE_MSK,
-				 AD7768_CH_MODE_FILTER_TYPE_WIDEBAND);
-	ret = regmap_update_bits(st->regmap, AD7768_REG_CH_MODE(0),
-				 AD7768_CH_MODE_FILTER_TYPE_MSK,
-				 mode_config);
-	if (ret)
-		return ret;
-
-	ret = regmap_write(st->regmap, AD7768_REG_CH_MODE_SEL, 0);
-	if (ret)
-		return ret;
-
-	ret = ad7768_set_clk_divs(st, default_freq);
-	if (ret)
-		return ret;
-
-	return ad7768_sync(st);
+	return 0;
 }
 
 static int ad7768_parse_config(struct iio_dev *indio_dev,
@@ -787,7 +882,11 @@ static int ad7768_parse_config(struct iio_dev *indio_dev,
 
 	ad7768_set_available_sampling_freqs(st);
 
-	return ad7768_configure_capture(st);
+	ret = ad7768_init_sampling_freqs(st);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to set power mode\n");
+
+	return 0;
 }
 
 static int ad7768_reset(struct ad7768_state *st)

-- 
2.43.0