[PATCH v2 3/3] ASoC: SDCA: Add port constraints into sdca_asoc_set_constraints()

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

Currently the SDCA code constrains the params based on the DisCo for
the SDCA Terminal used. However, it ignores the properties of the
SoundWire port, which in some cases may further constrain the allowed
params. Add additional constraints to apply these restrictions.

Some minor additional refactoring is required since port constraints
should be applied for both Input and Output Terminals, whereas
currently Cluster constraints are only handled for Input Terminals.

Signed-off-by: Charles Keepax <ckeepax@opensource.cirrus.com>
---

Change since v1:
 - Use BIT macro where appropriate
 - Correct GENMASK usage
 - Add missing array indexing
 - Correct channel masking, SDCA uses indexes not a count of channels
 - Reorder the priority of SDCA properties, it makes sense to
   consistently move from most specific to least specific
 - Handle the case of the word length not being specified, as it is
   optional the SDCA specification
 - Add a static_assert to check the constraint array is large enough for
   the word length mask
 - Some knock on changes from the new first patch in the series

 include/sound/sdca_asoc.h            |   4 +-
 sound/soc/sdca/sdca_asoc.c           | 108 +++++++++++++++++++++++----
 sound/soc/sdca/sdca_class_function.c |   3 +-
 3 files changed, 100 insertions(+), 15 deletions(-)

diff --git a/include/sound/sdca_asoc.h b/include/sound/sdca_asoc.h
index 236a441ec9d89..26740f69f2bee 100644
--- a/include/sound/sdca_asoc.h
+++ b/include/sound/sdca_asoc.h
@@ -12,6 +12,7 @@
 
 struct device;
 struct regmap;
+struct sdw_slave;
 struct sdca_function_data;
 struct sdca_pde_delay;
 struct snd_ctl_elem_value;
@@ -91,7 +92,8 @@ int sdca_asoc_populate_rate_format(struct device *dev,
 
 int sdca_asoc_alloc_constraints(struct snd_soc_dai *dai);
 int sdca_asoc_free_constraints(struct snd_soc_dai *dai);
-int sdca_asoc_set_constraints(struct device *dev, struct regmap *regmap,
+int sdca_asoc_set_constraints(struct device *dev, struct sdw_slave *sdw,
+			      struct regmap *regmap,
 			      struct sdca_function_data *function,
 			      struct snd_pcm_substream *substream,
 			      struct snd_soc_dai *dai);
diff --git a/sound/soc/sdca/sdca_asoc.c b/sound/soc/sdca/sdca_asoc.c
index be1b4c0075796..d1b40c6ccae2b 100644
--- a/sound/soc/sdca/sdca_asoc.c
+++ b/sound/soc/sdca/sdca_asoc.c
@@ -13,11 +13,13 @@
 #include <linux/delay.h>
 #include <linux/dev_printk.h>
 #include <linux/device.h>
+#include <linux/limits.h>
 #include <linux/minmax.h>
 #include <linux/module.h>
 #include <linux/overflow.h>
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
+#include <linux/soundwire/sdw.h>
 #include <linux/soundwire/sdw_registers.h>
 #include <linux/string_helpers.h>
 #include <linux/types.h>
@@ -1435,6 +1437,7 @@ int sdca_asoc_populate_component(struct device *dev,
 EXPORT_SYMBOL_NS(sdca_asoc_populate_component, "SND_SOC_SDCA");
 
 struct sdca_asoc_constraints {
+	struct snd_pcm_hw_constraint_list width;
 	struct snd_pcm_hw_constraint_list channels;
 };
 
@@ -1507,9 +1510,57 @@ static int get_cluster_mask(struct device *dev, struct sdca_function_data *funct
 	return 0;
 }
 
+static int get_port_masks(struct sdw_slave *sdw, unsigned int port,
+			  struct snd_pcm_substream *substream,
+			  unsigned int *word_mask, unsigned int *ch_mask)
+{
+	struct sdw_dpn_prop *prop;
+	unsigned int nprop;
+	int i, j;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		prop = sdw->prop.sink_dpn_prop;
+		nprop = hweight32(sdw->prop.sink_ports);
+	} else {
+		prop = sdw->prop.src_dpn_prop;
+		nprop = hweight32(sdw->prop.source_ports);
+	}
+
+	*word_mask = 0;
+	*ch_mask = 0;
+
+	for (i = 0; i < nprop; i++) {
+		if (prop[i].num != port)
+			continue;
+
+		if (prop[i].num_words) {
+			for (j = 0; j < prop[i].num_words; j++)
+				*word_mask |= BIT(prop[i].words[j] - 1);
+		} else {
+			*word_mask = GENMASK(MIN(31, prop[i].max_word - 1),
+					     MAX(1, prop[i].min_word) - 1);
+		}
+
+		/* Only handle the number of channels for constraints. */
+		if (prop[i].num_ch_combinations) {
+			for (j = 0; j < prop[i].num_ch_combinations; j++)
+				*ch_mask |= BIT(hweight32(prop[i].ch_combinations[j]) - 1);
+		} else if (prop[i].num_channels) {
+			*ch_mask = GENMASK(prop[i].num_channels - 1, 0);
+		} else {
+			*ch_mask = GENMASK(prop[i].max_ch - prop[i].min_ch, 0);
+		}
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
 /**
  * sdca_asoc_set_constraints - constrain channels available on a DAI
  * @dev: Pointer to the device, used for error messages.
+ * @sdw: Pointer to the SoundWire peripheral device.
  * @regmap: Pointer to the Function register map.
  * @function: Pointer to the Function information.
  * @substream: Pointer to the PCM substream.
@@ -1519,42 +1570,73 @@ static int get_cluster_mask(struct device *dev, struct sdca_function_data *funct
  *
  * Return: Returns zero on success, and a negative error code on failure.
  */
-int sdca_asoc_set_constraints(struct device *dev, struct regmap *regmap,
+int sdca_asoc_set_constraints(struct device *dev, struct sdw_slave *sdw,
+			      struct regmap *regmap,
 			      struct sdca_function_data *function,
 			      struct snd_pcm_substream *substream,
 			      struct snd_soc_dai *dai)
 {
-	static const unsigned int channel_list[] = {
+	static const unsigned int constraint_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_asoc_constraints *constraints = snd_soc_dai_to_priv(dai);
 	struct sdca_entity *entity = &function->entities[dai->id];
+	unsigned int port_ch_mask, port_width_mask;
 	unsigned int cluster_ch_mask;
+	int port;
 	int ret;
 
-	static_assert(ARRAY_SIZE(channel_list) == SDCA_MAX_CHANNEL_COUNT);
+	static_assert(ARRAY_SIZE(constraint_list) <= sizeof(port_width_mask) * BITS_PER_BYTE);
+	static_assert(ARRAY_SIZE(constraint_list) == SDCA_MAX_CHANNEL_COUNT);
 	static_assert(sizeof(cluster_ch_mask) * BITS_PER_BYTE >= SDCA_MAX_CHANNEL_COUNT);
 
-	if (entity->type != SDCA_ENTITY_TYPE_IT)
-		return 0;
+	/* FIXME: Currently only a single unshared port is supported */
+	port = sdca_asoc_get_port(dev, regmap, function, dai);
+	if (port < 0)
+		return port;
 
-	ret = get_cluster_mask(dev, function, entity, &cluster_ch_mask);
-	if (ret)
+	ret = get_port_masks(sdw, port, substream, &port_width_mask, &port_ch_mask);
+	if (ret < 0) {
+		dev_err(dev, "%s: no properties for port %d\n", entity->label, port);
 		return ret;
+	}
+
+	/* FIXME: OTs should inherit cluster information */
+	if (entity->type == SDCA_ENTITY_TYPE_IT) {
+		ret = get_cluster_mask(dev, function, entity, &cluster_ch_mask);
+		if (ret)
+			return ret;
+	} else {
+		cluster_ch_mask = UINT_MAX;
+	}
+
+	constraints->channels.count = ARRAY_SIZE(constraint_list);
+	constraints->channels.list = constraint_list;
+	constraints->channels.mask = cluster_ch_mask & port_ch_mask;
 
-	dev_dbg(dev, "%s: set channel constraint mask: %#x\n",
-		entity->label, cluster_ch_mask);
+	constraints->width.count = ARRAY_SIZE(constraint_list);
+	constraints->width.list = constraint_list;
+	constraints->width.mask = port_width_mask;
 
-	constraints->channels.count = ARRAY_SIZE(channel_list);
-	constraints->channels.list = channel_list;
-	constraints->channels.mask = cluster_ch_mask;
+	dev_dbg(dev, "%s: set constraint masks, channel: %#x, width: %#x\n",
+		entity->label, constraints->channels.mask, constraints->width.mask);
 
 	ret = snd_pcm_hw_constraint_list(substream->runtime, 0,
 					 SNDRV_PCM_HW_PARAM_CHANNELS,
 					 &constraints->channels);
 	if (ret) {
-		dev_err(dev, "%s: failed to add constraint: %d\n", entity->label, ret);
+		dev_err(dev, "%s: failed to add channel constraint: %d\n",
+			entity->label, ret);
+		return ret;
+	}
+
+	ret = snd_pcm_hw_constraint_list(substream->runtime, 0,
+					 SNDRV_PCM_HW_PARAM_SAMPLE_BITS,
+					 &constraints->width);
+	if (ret) {
+		dev_err(dev, "%s: failed to add word constraint: %d\n",
+			entity->label, ret);
 		return ret;
 	}
 
diff --git a/sound/soc/sdca/sdca_class_function.c b/sound/soc/sdca/sdca_class_function.c
index 6406c9d0f6119..abccc63640c68 100644
--- a/sound/soc/sdca/sdca_class_function.c
+++ b/sound/soc/sdca/sdca_class_function.c
@@ -121,8 +121,9 @@ static int class_function_startup(struct snd_pcm_substream *substream,
 				  struct snd_soc_dai *dai)
 {
 	struct class_function_drv *drv = snd_soc_component_get_drvdata(dai->component);
+	struct sdw_slave *sdw = dev_to_sdw_dev(drv->dev->parent);
 
-	return sdca_asoc_set_constraints(drv->dev, drv->regmap, drv->function,
+	return sdca_asoc_set_constraints(drv->dev, sdw, drv->regmap, drv->function,
 					 substream, dai);
 }
 
-- 
2.47.3