[PATCH 2/2] ASoC: sdw_utils: cs_amp: Assign non-overlapping TDM masks for each codec on a bus

[Bard Liao <yung-chuan.liao@xxxxxxxxxxxxxxx>]

From: Richard Fitzgerald <rf@xxxxxxxxxxxxxxxxxxxxx>

Use snd_soc_dai_set_tdm_slot() on capture DAIs to prevent multiple
aggregated amps from trying to send data at the same time.

When the capture DAIs of multiple amps on a bus are aggregated they will
all be sharing the same bit slots for transmitted audio. This would lead
to bus errors if all channels on all amps were enabled, because multiple
amps would be trying to send data at the same time.

To prevent this, the available channels are divided between the amps on a
bus so that only one amp will be sending data for each channel position.

A CS35L56 has 4 TX channels, which must be split between all the amps on a
bus so that no two amps are using the same channel. This is done simply by
dividing by the number of amps on the bus, so that 1 amp can use all
4 channels, 2 amps can use 2 channels each, and 3 or 4 amps can only use
1 channel each.

The amps are usually aggregated across multiple SoundWire buses. In this
case there will be multiple cpu DAIs in the dailink. The channel mapping
is used to determine which amps are on each bus. The allocation of the
4 channels is done separately for each bus (only amps on the same bus can
interfere with each other).

Signed-off-by: Richard Fitzgerald <rf@xxxxxxxxxxxxxxxxxxxxx>
Reviewed-by: Péter Ujfalusi <peter.ujfalusi@xxxxxxxxxxxxxxx>
Reviewed-by: Liam Girdwood <liam.r.girdwood@xxxxxxxxx>
Reviewed-by: Ranjani Sridharan <ranjani.sridharan@xxxxxxxxxxxxxxx>
Signed-off-by: Bard Liao <yung-chuan.liao@xxxxxxxxxxxxxxx>
---
 include/sound/soc_sdw_utils.h        |  2 ++
 sound/soc/sdw_utils/soc_sdw_cs_amp.c | 46 ++++++++++++++++++++++++++++
 sound/soc/sdw_utils/soc_sdw_utils.c  |  1 +
 3 files changed, 49 insertions(+)

diff --git a/include/sound/soc_sdw_utils.h b/include/sound/soc_sdw_utils.h
index 0e82598e10af..36a4a1e1d8ca 100644
--- a/include/sound/soc_sdw_utils.h
+++ b/include/sound/soc_sdw_utils.h
@@ -224,6 +224,8 @@ int asoc_sdw_cs_amp_init(struct snd_soc_card *card,
 			 struct snd_soc_dai_link *dai_links,
 			 struct asoc_sdw_codec_info *info,
 			 bool playback);
+int asoc_sdw_cs_spk_feedback_rtd_init(struct snd_soc_pcm_runtime *rtd,
+				      struct snd_soc_dai *dai);
 
 /* MAXIM codec support */
 int asoc_sdw_maxim_init(struct snd_soc_card *card,
diff --git a/sound/soc/sdw_utils/soc_sdw_cs_amp.c b/sound/soc/sdw_utils/soc_sdw_cs_amp.c
index 58b059b68016..9146a421e02c 100644
--- a/sound/soc/sdw_utils/soc_sdw_cs_amp.c
+++ b/sound/soc/sdw_utils/soc_sdw_cs_amp.c
@@ -15,6 +15,7 @@
 #include <sound/soc_sdw_utils.h>
 
 #define CODEC_NAME_SIZE	8
+#define CS_AMP_CHANNELS_PER_AMP	4
 
 int asoc_sdw_cs_spk_rtd_init(struct snd_soc_pcm_runtime *rtd, struct snd_soc_dai *dai)
 {
@@ -48,6 +49,51 @@ int asoc_sdw_cs_spk_rtd_init(struct snd_soc_pcm_runtime *rtd, struct snd_soc_dai
 }
 EXPORT_SYMBOL_NS(asoc_sdw_cs_spk_rtd_init, SND_SOC_SDW_UTILS);
 
+int asoc_sdw_cs_spk_feedback_rtd_init(struct snd_soc_pcm_runtime *rtd, struct snd_soc_dai *dai)
+{
+	const struct snd_soc_dai_link *dai_link = rtd->dai_link;
+	const struct snd_soc_dai_link_ch_map *ch_map;
+	const struct snd_soc_dai_link_component *codec_dlc;
+	struct snd_soc_dai *codec_dai;
+	u8 ch_slot[8] = {};
+	unsigned int amps_per_bus, ch_per_amp, mask;
+	int i, ret;
+
+	WARN_ON(dai_link->num_cpus > ARRAY_SIZE(ch_slot));
+
+	/*
+	 * CS35L56 has 4 TX channels. When the capture is aggregated the
+	 * same bus slots will be allocated to all the amps on a bus. Only
+	 * one amp on that bus can be transmitting in each slot so divide
+	 * the available 4 slots between all the amps on a bus.
+	 */
+	amps_per_bus = dai_link->num_codecs / dai_link->num_cpus;
+	if ((amps_per_bus == 0) || (amps_per_bus > CS_AMP_CHANNELS_PER_AMP)) {
+		dev_err(rtd->card->dev, "Illegal num_codecs:%u / num_cpus:%u\n",
+			dai_link->num_codecs, dai_link->num_cpus);
+		return -EINVAL;
+	}
+
+	ch_per_amp = CS_AMP_CHANNELS_PER_AMP / amps_per_bus;
+
+	for_each_rtd_ch_maps(rtd, i, ch_map) {
+		codec_dlc = snd_soc_link_to_codec(rtd->dai_link, i);
+		codec_dai = snd_soc_find_dai(codec_dlc);
+		mask = GENMASK(ch_per_amp - 1, 0) << ch_slot[ch_map->cpu];
+
+		ret = snd_soc_dai_set_tdm_slot(codec_dai, 0, mask, 4, 32);
+		if (ret < 0) {
+			dev_err(rtd->card->dev, "Failed to set TDM slot:%d\n", ret);
+			return ret;
+		}
+
+		ch_slot[ch_map->cpu] += ch_per_amp;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_NS(asoc_sdw_cs_spk_feedback_rtd_init, SND_SOC_SDW_UTILS);
+
 int asoc_sdw_cs_amp_init(struct snd_soc_card *card,
 			 struct snd_soc_dai_link *dai_links,
 			 struct asoc_sdw_codec_info *info,
diff --git a/sound/soc/sdw_utils/soc_sdw_utils.c b/sound/soc/sdw_utils/soc_sdw_utils.c
index 111e00e916c5..375b46c5b429 100644
--- a/sound/soc/sdw_utils/soc_sdw_utils.c
+++ b/sound/soc/sdw_utils/soc_sdw_utils.c
@@ -504,6 +504,7 @@ struct asoc_sdw_codec_info codec_info_list[] = {
 				.dai_name = "cs35l56-sdw1c",
 				.dai_type = SOC_SDW_DAI_TYPE_AMP,
 				.dailink = {SOC_SDW_UNUSED_DAI_ID, SOC_SDW_AMP_IN_DAI_ID},
+				.rtd_init = asoc_sdw_cs_spk_feedback_rtd_init,
 			},
 		},
 		.dai_num = 2,
-- 
2.43.0