[PATCH v2 04/17] ASoC: pcm: Honor subformat when configuring runtime

[Cezary Rojewski <cezary.rojewski@xxxxxxxxx>]

Subformat options are ignored when setting up hardware parameters and
assigning PCM stream capabilities. Account for them to allow for
granular format selection.

With introduction of subformats, copying hardware parameters becomes
a non-trivial operation. Implement a helper function to make things
simple.

Signed-off-by: Cezary Rojewski <cezary.rojewski@xxxxxxxxx>
---
 include/sound/soc.h |  1 +
 sound/soc/soc-pcm.c | 35 ++++++++++++++++++++++++++++++++---
 2 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/include/sound/soc.h b/include/sound/soc.h
index 81ed08c5c67d..0d3158a7715e 100644
--- a/include/sound/soc.h
+++ b/include/sound/soc.h
@@ -620,6 +620,7 @@ enum snd_soc_trigger_order {
 struct snd_soc_pcm_stream {
 	const char *stream_name;
 	u64 formats;			/* SNDRV_PCM_FMTBIT_* */
+	const struct snd_pcm_subformat *subformats;
 	unsigned int rates;		/* SNDRV_PCM_RATE_* */
 	unsigned int rate_min;		/* min rate */
 	unsigned int rate_max;		/* max rate */
diff --git a/sound/soc/soc-pcm.c b/sound/soc/soc-pcm.c
index 54704250c0a2..b57795feb5f8 100644
--- a/sound/soc/soc-pcm.c
+++ b/sound/soc/soc-pcm.c
@@ -336,9 +336,7 @@ bool snd_soc_runtime_ignore_pmdown_time(struct snd_soc_pcm_runtime *rtd)
 int snd_soc_set_runtime_hwparams(struct snd_pcm_substream *substream,
 	const struct snd_pcm_hardware *hw)
 {
-	substream->runtime->hw = *hw;
-
-	return 0;
+	return snd_pcm_hw_copy(&substream->runtime->hw, hw);
 }
 EXPORT_SYMBOL_GPL(snd_soc_set_runtime_hwparams);
 
@@ -554,6 +552,33 @@ static void soc_pcm_hw_update_format(struct snd_pcm_hardware *hw,
 	hw->formats &= p->formats;
 }
 
+static void soc_pcm_hw_update_subformat(struct snd_pcm_hardware *hw,
+					struct snd_soc_pcm_stream *p)
+{
+	const struct snd_pcm_subformat *sf2;
+	struct snd_pcm_subformat *sf1;
+	u32 last = 0;
+
+	if (!hw->subformats || !p->subformats)
+		return;
+
+	for (sf1 = hw->subformats; sf1->mask; sf1++)
+		last++;
+
+	for (sf1 = hw->subformats; sf1->mask; sf1++) {
+		for (sf2 = p->subformats; sf2->mask; sf2++) {
+			if (sf1->format != sf2->format)
+				continue;
+
+			sf1->mask &= sf2->mask;
+			if (!sf1->mask) {
+				swap(*sf1, hw->subformats[last]);
+				last--;
+			}
+		}
+	}
+}
+
 /**
  * snd_soc_runtime_calc_hw() - Calculate hw limits for a PCM stream
  * @rtd: ASoC PCM runtime
@@ -592,6 +617,7 @@ int snd_soc_runtime_calc_hw(struct snd_soc_pcm_runtime *rtd,
 		soc_pcm_hw_update_chan(hw, cpu_stream);
 		soc_pcm_hw_update_rate(hw, cpu_stream);
 		soc_pcm_hw_update_format(hw, cpu_stream);
+		soc_pcm_hw_update_subformat(hw, cpu_stream);
 	}
 	cpu_chan_min = hw->channels_min;
 	cpu_chan_max = hw->channels_max;
@@ -613,6 +639,7 @@ int snd_soc_runtime_calc_hw(struct snd_soc_pcm_runtime *rtd,
 		soc_pcm_hw_update_chan(hw, codec_stream);
 		soc_pcm_hw_update_rate(hw, codec_stream);
 		soc_pcm_hw_update_format(hw, codec_stream);
+		soc_pcm_hw_update_subformat(hw, codec_stream);
 	}
 
 	/* Verify both a valid CPU DAI and a valid CODEC DAI were found */
@@ -1698,6 +1725,7 @@ static void dpcm_runtime_setup_fe(struct snd_pcm_substream *substream)
 		soc_pcm_hw_update_rate(hw, cpu_stream);
 		soc_pcm_hw_update_chan(hw, cpu_stream);
 		soc_pcm_hw_update_format(hw, cpu_stream);
+		soc_pcm_hw_update_subformat(hw, cpu_stream);
 	}
 
 }
@@ -1735,6 +1763,7 @@ static void dpcm_runtime_setup_be_format(struct snd_pcm_substream *substream)
 			codec_stream = snd_soc_dai_get_pcm_stream(dai, stream);
 
 			soc_pcm_hw_update_format(hw, codec_stream);
+			soc_pcm_hw_update_subformat(hw, codec_stream);
 		}
 	}
 }
-- 
2.25.1