[PATCH 5/7] ASoC: SDCA: Add helper to add DAI constraints

[Charles Keepax <ckeepax@opensource.cirrus.com>]

Currently the core SDCA code simply creates a place holder available
channels from 1 to SDCA_MAX_CHANNEL_COUNT. Add a helper function
that will constrain the number of channels based on the actual
available SDCA Clusters in DisCo. Currently this code only handles
Input Terminal Entities as they directly specify the Cluster. More
work will be required later for Output Terminals which inherit their
Cluster. Typically this new helper would be called from the DAIs
startup callback.

Signed-off-by: Charles Keepax <ckeepax@opensource.cirrus.com>
---
 include/sound/sdca_asoc.h       | 10 ++++
 include/sound/sdca_function.h   | 12 ++++
 sound/soc/sdca/sdca_asoc.c      | 99 +++++++++++++++++++++++++++++++++
 sound/soc/sdca/sdca_functions.c | 18 ++++++
 4 files changed, 139 insertions(+)

diff --git a/include/sound/sdca_asoc.h b/include/sound/sdca_asoc.h
index 9121531f0826..bbf146e4fcea 100644
--- a/include/sound/sdca_asoc.h
+++ b/include/sound/sdca_asoc.h
@@ -11,9 +11,12 @@
 #define __SDCA_ASOC_H__
 
 struct device;
+struct regmap;
 struct sdca_function_data;
 struct snd_kcontrol_new;
+struct snd_pcm_substream;
 struct snd_soc_component_driver;
+struct snd_soc_dai;
 struct snd_soc_dai_driver;
 struct snd_soc_dai_ops;
 struct snd_soc_dapm_route;
@@ -39,4 +42,11 @@ int sdca_asoc_populate_component(struct device *dev,
 				 struct snd_soc_dai_driver **dai_drv, int *num_dai_drv,
 				 const struct snd_soc_dai_ops *ops);
 
+int sdca_asoc_set_constraints(struct device *dev, struct regmap *regmap,
+			      struct sdca_function_data *function,
+			      struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai);
+void sdca_asoc_free_constraints(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai);
+
 #endif // __SDCA_ASOC_H__
diff --git a/include/sound/sdca_function.h b/include/sound/sdca_function.h
index 543c09e99ab1..3bde07409bf3 100644
--- a/include/sound/sdca_function.h
+++ b/include/sound/sdca_function.h
@@ -1268,6 +1268,15 @@ struct sdca_cluster {
 	struct sdca_channel *channels;
 };
 
+/**
+ * enum sdca_cluster_range - SDCA Range column definitions for ClusterIndex
+ */
+enum sdca_cluster_range {
+	SDCA_CLUSTER_BYTEINDEX				= 0,
+	SDCA_CLUSTER_CLUSTERID				= 1,
+	SDCA_CLUSTER_NCOLS				= 2,
+};
+
 /**
  * struct sdca_function_data - top-level information for one SDCA function
  * @desc: Pointer to short descriptor from initial parsing.
@@ -1326,5 +1335,8 @@ struct sdca_control_range *sdca_control_find_range(struct device *dev,
 struct sdca_control_range *sdca_selector_find_range(struct device *dev,
 						    struct sdca_entity *entity,
 						    int sel, int cols, int rows);
+struct sdca_cluster *sdca_id_find_cluster(struct device *dev,
+					  struct sdca_function_data *function,
+					  const int id);
 
 #endif
diff --git a/sound/soc/sdca/sdca_asoc.c b/sound/soc/sdca/sdca_asoc.c
index 11c9b3b935fc..1a0149287584 100644
--- a/sound/soc/sdca/sdca_asoc.c
+++ b/sound/soc/sdca/sdca_asoc.c
@@ -7,16 +7,20 @@
  * https://www.mipi.org/mipi-sdca-v1-0-download
  */
 
+#include <linux/bits.h>
 #include <linux/bitmap.h>
+#include <linux/build_bug.h>
 #include <linux/delay.h>
 #include <linux/dev_printk.h>
 #include <linux/device.h>
 #include <linux/minmax.h>
 #include <linux/module.h>
 #include <linux/overflow.h>
+#include <linux/regmap.h>
 #include <linux/soundwire/sdw_registers.h>
 #include <linux/string_helpers.h>
 #include <sound/control.h>
+#include <sound/pcm.h>
 #include <sound/sdca.h>
 #include <sound/sdca_asoc.h>
 #include <sound/sdca_function.h>
@@ -1269,3 +1273,98 @@ int sdca_asoc_populate_component(struct device *dev,
 	return 0;
 }
 EXPORT_SYMBOL_NS(sdca_asoc_populate_component, "SND_SOC_SDCA");
+
+/**
+ * sdca_asoc_set_constraints - constrain channels available on a DAI
+ * @dev: Pointer to the device, used for error messages.
+ * @regmap: Pointer to the Function register map.
+ * @function: Pointer to the Function information.
+ * @substream: Pointer to the PCM substream.
+ * @dai: Pointer to the ASoC DAI.
+ *
+ * Typically called from startup().
+ *
+ * Return: Returns zero on success, and a negative error code on failure.
+ */
+int sdca_asoc_set_constraints(struct device *dev, struct regmap *regmap,
+			      struct sdca_function_data *function,
+			      struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai)
+{
+	static const unsigned int channel_list[] = {
+		 1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16,
+		17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+	};
+	struct sdca_entity *entity = &function->entities[dai->id];
+	struct snd_pcm_hw_constraint_list *constraint;
+	struct sdca_control_range *range;
+	struct sdca_control *control;
+	unsigned int channel_mask = 0;
+	int i, ret;
+
+	static_assert(ARRAY_SIZE(channel_list) == SDCA_MAX_CHANNEL_COUNT);
+	static_assert(sizeof(channel_mask) * BITS_PER_BYTE >= SDCA_MAX_CHANNEL_COUNT);
+
+	if (entity->type != SDCA_ENTITY_TYPE_IT)
+		return 0;
+
+	control = sdca_selector_find_control(dev, entity, SDCA_CTL_IT_CLUSTERINDEX);
+	if (!control)
+		return -EINVAL;
+
+	range = sdca_control_find_range(dev, entity, control, SDCA_CLUSTER_NCOLS, 0);
+	if (!range)
+		return -EINVAL;
+
+	for (i = 0; i < range->rows; i++) {
+		int clusterid = sdca_range(range, SDCA_CLUSTER_CLUSTERID, i);
+		struct sdca_cluster *cluster;
+
+		cluster = sdca_id_find_cluster(dev, function, clusterid);
+		if (!cluster)
+			return -ENODEV;
+
+		channel_mask |= (1 << (cluster->num_channels - 1));
+	}
+
+	dev_dbg(dev, "%s: set channel constraint mask: %#x\n",
+		entity->label, channel_mask);
+
+	constraint = kzalloc(sizeof(*constraint), GFP_KERNEL);
+	if (!constraint)
+		return -ENOMEM;
+
+	constraint->count = ARRAY_SIZE(channel_list);
+	constraint->list = channel_list;
+	constraint->mask = channel_mask;
+
+	ret = snd_pcm_hw_constraint_list(substream->runtime, 0,
+					 SNDRV_PCM_HW_PARAM_CHANNELS,
+					 constraint);
+	if (ret) {
+		dev_err(dev, "%s: failed to add constraint: %d\n", entity->label, ret);
+		kfree(constraint);
+		return ret;
+	}
+
+	dai->priv = constraint;
+
+	return 0;
+}
+EXPORT_SYMBOL_NS(sdca_asoc_set_constraints, "SND_SOC_SDCA");
+
+/**
+ * sdca_asoc_free_constraints - free constraint allocations
+ * @substream: Pointer to the PCM substream.
+ * @dai: Pointer to the ASoC DAI.
+ *
+ * Typically called from shutdown().
+ */
+void sdca_asoc_free_constraints(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	struct snd_pcm_hw_constraint_list *constraint = dai->priv;
+
+	kfree(constraint);
+}
+EXPORT_SYMBOL_NS(sdca_asoc_free_constraints, "SND_SOC_SDCA");
diff --git a/sound/soc/sdca/sdca_functions.c b/sound/soc/sdca/sdca_functions.c
index ca965e8a4c46..0019c72b4904 100644
--- a/sound/soc/sdca/sdca_functions.c
+++ b/sound/soc/sdca/sdca_functions.c
@@ -1991,6 +1991,24 @@ struct sdca_control_range *sdca_selector_find_range(struct device *dev,
 }
 EXPORT_SYMBOL_NS(sdca_selector_find_range, "SND_SOC_SDCA");
 
+struct sdca_cluster *sdca_id_find_cluster(struct device *dev,
+					  struct sdca_function_data *function,
+					  const int id)
+{
+	int i;
+
+	for (i = 0; i < function->num_clusters; i++) {
+		struct sdca_cluster *cluster = &function->clusters[i];
+
+		if (cluster->id == id)
+			return cluster;
+	}
+
+	dev_err(dev, "%s: cluster %#x: missing\n", function->desc->name, id);
+	return NULL;
+}
+EXPORT_SYMBOL_NS(sdca_id_find_cluster, "SND_SOC_SDCA");
+
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_DESCRIPTION("SDCA library");
 MODULE_IMPORT_NS("SND_SOC_SDCA_HID");
-- 
2.39.5