ASoC: add N cpus to M codecs dai link support

Merge series from Bard Liao <yung-chuan.liao@linux.intel.com>:

Currently, ASoC supports dailinks with the following mappings:
1 cpu DAI to N codec DAIs
N cpu DAIs to N codec DAIs
But the mapping between N cpu DAIs and M codec DAIs is not supported.
The reason is that we didn't have a mechanism to map cpu and codec DAIs

This series suggests a new snd_soc_dai_link_codec_ch_map struct in
struct snd_soc_dai_link{} which provides codec DAI to cpu DAI mapping
information used to implement N cpu DAIs to M codec DAIs support.

And add the codec_ch_maps to SOF SoundWire machine driver.

6 files changed, 141 insertions, 5 deletions

diff --git a/include/sound/soc.h b/include/sound/soc.h
index 10e4ea0664af..1e48a1135844 100644
--- a/include/sound/soc.h
+++ b/include/sound/soc.h
@@ -645,6 +645,11 @@ struct snd_soc_dai_link_component {
 	const char *dai_name;
 };
 
+struct snd_soc_dai_link_codec_ch_map {
+	unsigned int connected_cpu_id;
+	unsigned int ch_mask;
+};
+
 struct snd_soc_dai_link {
 	/* config - must be set by machine driver */
 	const char *name;			/* Codec name */
@@ -673,6 +678,7 @@ struct snd_soc_dai_link {
 	struct snd_soc_dai_link_component *codecs;
 	unsigned int num_codecs;
 
+	struct snd_soc_dai_link_codec_ch_map *codec_ch_maps;
 	/*
 	 * You MAY specify the link's platform/PCM/DMA driver, either by
 	 * device name, or by DT/OF node, but not both. Some forms of link
diff --git a/sound/soc/intel/boards/sof_sdw.c b/sound/soc/intel/boards/sof_sdw.c
index d942696b36cd..f2f56150e1da 100644
--- a/sound/soc/intel/boards/sof_sdw.c
+++ b/sound/soc/intel/boards/sof_sdw.c
@@ -560,6 +560,55 @@ int sdw_trigger(struct snd_pcm_substream *substream, int cmd)
 	return ret;
 }
 
+int sdw_hw_params(struct snd_pcm_substream *substream,
+		  struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = asoc_substream_to_rtd(substream);
+	int ch = params_channels(params);
+	struct snd_soc_dai *codec_dai;
+	struct snd_soc_dai *cpu_dai;
+	unsigned int ch_mask;
+	int num_codecs;
+	int step;
+	int i;
+	int j;
+
+	if (!rtd->dai_link->codec_ch_maps)
+		return 0;
+
+	/* Identical data will be sent to all codecs in playback */
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		ch_mask = GENMASK(ch - 1, 0);
+		step = 0;
+	} else {
+		num_codecs = rtd->dai_link->num_codecs;
+
+		if (ch < num_codecs || ch % num_codecs != 0) {
+			dev_err(rtd->dev, "Channels number %d is invalid when codec number = %d\n",
+				ch, num_codecs);
+			return -EINVAL;
+		}
+
+		ch_mask = GENMASK(ch / num_codecs - 1, 0);
+		step = hweight_long(ch_mask);
+
+	}
+
+	/*
+	 * The captured data will be combined from each cpu DAI if the dai
+	 * link has more than one codec DAIs. Set codec channel mask and
+	 * ASoC will set the corresponding channel numbers for each cpu dai.
+	 */
+	for_each_rtd_cpu_dais(rtd, i, cpu_dai) {
+		for_each_rtd_codec_dais(rtd, j, codec_dai) {
+			if (rtd->dai_link->codec_ch_maps[j].connected_cpu_id != i)
+				continue;
+			rtd->dai_link->codec_ch_maps[j].ch_mask = ch_mask << (j * step);
+		}
+	}
+	return 0;
+}
+
 int sdw_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_soc_pcm_runtime *rtd = asoc_substream_to_rtd(substream);
@@ -588,6 +637,7 @@ static const struct snd_soc_ops sdw_ops = {
 	.startup = sdw_startup,
 	.prepare = sdw_prepare,
 	.trigger = sdw_trigger,
+	.hw_params = sdw_hw_params,
 	.hw_free = sdw_hw_free,
 	.shutdown = sdw_shutdown,
 };
@@ -1281,6 +1331,17 @@ static int get_slave_info(const struct snd_soc_acpi_link_adr *adr_link,
 	return 0;
 }
 
+static void set_dailink_map(struct snd_soc_dai_link_codec_ch_map *sdw_codec_ch_maps,
+			    int codec_num, int cpu_num)
+{
+	int step;
+	int i;
+
+	step = codec_num / cpu_num;
+	for (i = 0; i < codec_num; i++)
+		sdw_codec_ch_maps[i].connected_cpu_id = i / step;
+}
+
 static const char * const type_strings[] = {"SimpleJack", "SmartAmp", "SmartMic"};
 
 static int create_sdw_dailink(struct snd_soc_card *card,
@@ -1357,6 +1418,7 @@ static int create_sdw_dailink(struct snd_soc_card *card,
 
 	cpu_dai_index = *cpu_id;
 	for_each_pcm_streams(stream) {
+		struct snd_soc_dai_link_codec_ch_map *sdw_codec_ch_maps;
 		char *name, *cpu_name;
 		int playback, capture;
 		static const char * const sdw_stream_name[] = {
@@ -1375,6 +1437,11 @@ static int create_sdw_dailink(struct snd_soc_card *card,
 			return -EINVAL;
 		}
 
+		sdw_codec_ch_maps = devm_kcalloc(dev, codec_num,
+						 sizeof(*sdw_codec_ch_maps), GFP_KERNEL);
+		if (!sdw_codec_ch_maps)
+			return -ENOMEM;
+
 		/* create stream name according to first link id */
 		if (append_dai_type) {
 			name = devm_kasprintf(dev, GFP_KERNEL,
@@ -1435,6 +1502,8 @@ static int create_sdw_dailink(struct snd_soc_card *card,
 		 */
 		dai_links[*link_index].nonatomic = true;
 
+		set_dailink_map(sdw_codec_ch_maps, codec_num, cpu_dai_num);
+		dai_links[*link_index].codec_ch_maps = sdw_codec_ch_maps;
 		ret = set_codec_init_func(card, link, dai_links + (*link_index)++,
 					  playback, group_id, adr_index, dai_index);
 		if (ret < 0) {
diff --git a/sound/soc/intel/boards/sof_sdw_common.h b/sound/soc/intel/boards/sof_sdw_common.h
index 64cfa5d1aceb..37402170d5f9 100644
--- a/sound/soc/intel/boards/sof_sdw_common.h
+++ b/sound/soc/intel/boards/sof_sdw_common.h
@@ -103,6 +103,8 @@ extern unsigned long sof_sdw_quirk;
 int sdw_startup(struct snd_pcm_substream *substream);
 int sdw_prepare(struct snd_pcm_substream *substream);
 int sdw_trigger(struct snd_pcm_substream *substream, int cmd);
+int sdw_hw_params(struct snd_pcm_substream *substream,
+		  struct snd_pcm_hw_params *params);
 int sdw_hw_free(struct snd_pcm_substream *substream);
 void sdw_shutdown(struct snd_pcm_substream *substream);
 
diff --git a/sound/soc/intel/boards/sof_sdw_maxim.c b/sound/soc/intel/boards/sof_sdw_maxim.c
index 8d40a83ad98e..414c4d8dac77 100644
--- a/sound/soc/intel/boards/sof_sdw_maxim.c
+++ b/sound/soc/intel/boards/sof_sdw_maxim.c
@@ -123,6 +123,7 @@ static const struct snd_soc_ops max_98373_sdw_ops = {
 	.startup = sdw_startup,
 	.prepare = mx8373_sdw_prepare,
 	.trigger = sdw_trigger,
+	.hw_params = sdw_hw_params,
 	.hw_free = mx8373_sdw_hw_free,
 	.shutdown = sdw_shutdown,
 };
diff --git a/sound/soc/soc-dapm.c b/sound/soc/soc-dapm.c
index b7b31d4e8ae8..3091e8160bad 100644
--- a/sound/soc/soc-dapm.c
+++ b/sound/soc/soc-dapm.c
@@ -4444,9 +4444,31 @@ void snd_soc_dapm_connect_dai_link_widgets(struct snd_soc_card *card)
 			for_each_rtd_codec_dais(rtd, i, codec_dai)
 				dapm_connect_dai_pair(card, rtd, codec_dai,
 						      asoc_rtd_to_cpu(rtd, i));
+		} else if (rtd->dai_link->num_codecs > rtd->dai_link->num_cpus) {
+			int cpu_id;
+
+			if (!rtd->dai_link->codec_ch_maps) {
+				dev_err(card->dev, "%s: no codec channel mapping table provided\n",
+					__func__);
+				continue;
+			}
+
+			for_each_rtd_codec_dais(rtd, i, codec_dai) {
+				cpu_id = rtd->dai_link->codec_ch_maps[i].connected_cpu_id;
+				if (cpu_id >= rtd->dai_link->num_cpus) {
+					dev_err(card->dev,
+						"%s: dai_link %s cpu_id %d too large, num_cpus is %d\n",
+						__func__, rtd->dai_link->name, cpu_id,
+						rtd->dai_link->num_cpus);
+					continue;
+				}
+				dapm_connect_dai_pair(card, rtd, codec_dai,
+						      asoc_rtd_to_cpu(rtd, cpu_id));
+			}
 		} else {
 			dev_err(card->dev,
-				"N cpus to M codecs link is not supported yet\n");
+				"%s: codec number %d < cpu number %d is not supported\n",
+				__func__, rtd->dai_link->num_codecs, rtd->dai_link->num_cpus);
 		}
 	}
 }
diff --git a/sound/soc/soc-pcm.c b/sound/soc/soc-pcm.c
index 799865a6eb56..60cfbe565759 100644
--- a/sound/soc/soc-pcm.c
+++ b/sound/soc/soc-pcm.c
@@ -1034,6 +1034,10 @@ static int __soc_pcm_hw_params(struct snd_soc_pcm_runtime *rtd,
 	}
 
 	for_each_rtd_cpu_dais(rtd, i, cpu_dai) {
+		struct snd_pcm_hw_params cpu_params;
+		unsigned int ch_mask = 0;
+		int j;
+
 		/*
 		 * Skip CPUs which don't support the current stream
 		 * type. See soc_pcm_init_runtime_hw() for more details
@@ -1041,13 +1045,32 @@ static int __soc_pcm_hw_params(struct snd_soc_pcm_runtime *rtd,
 		if (!snd_soc_dai_stream_valid(cpu_dai, substream->stream))
 			continue;
 
-		ret = snd_soc_dai_hw_params(cpu_dai, substream, params);
+		/* copy params for each cpu */
+		cpu_params = *params;
+
+		if (!rtd->dai_link->codec_ch_maps)
+			goto hw_params;
+		/*
+		 * construct cpu channel mask by combining ch_mask of each
+		 * codec which maps to the cpu.
+		 */
+		for_each_rtd_codec_dais(rtd, j, codec_dai) {
+			if (rtd->dai_link->codec_ch_maps[j].connected_cpu_id == i)
+				ch_mask |= rtd->dai_link->codec_ch_maps[j].ch_mask;
+		}
+
+		/* fixup cpu channel number */
+		if (ch_mask)
+			soc_pcm_codec_params_fixup(&cpu_params, ch_mask);
+
+hw_params:
+		ret = snd_soc_dai_hw_params(cpu_dai, substream, &cpu_params);
 		if (ret < 0)
 			goto out;
 
 		/* store the parameters for each DAI */
-		soc_pcm_set_dai_params(cpu_dai, params);
-		snd_soc_dapm_update_dai(substream, params, cpu_dai);
+		soc_pcm_set_dai_params(cpu_dai, &cpu_params);
+		snd_soc_dapm_update_dai(substream, &cpu_params, cpu_dai);
 	}
 
 	ret = snd_soc_pcm_component_hw_params(substream, params);
@@ -2789,9 +2812,22 @@ static int soc_get_playback_capture(struct snd_soc_pcm_runtime *rtd,
 				cpu_dai = asoc_rtd_to_cpu(rtd, 0);
 			} else if (dai_link->num_cpus == dai_link->num_codecs) {
 				cpu_dai = asoc_rtd_to_cpu(rtd, i);
+			} else if (rtd->dai_link->num_codecs > rtd->dai_link->num_cpus) {
+				int cpu_id;
+
+				if (!rtd->dai_link->codec_ch_maps) {
+					dev_err(rtd->card->dev, "%s: no codec channel mapping table provided\n",
+						__func__);
+					return -EINVAL;
+				}
+
+				cpu_id = rtd->dai_link->codec_ch_maps[i].connected_cpu_id;
+				cpu_dai = asoc_rtd_to_cpu(rtd, cpu_id);
 			} else {
 				dev_err(rtd->card->dev,
-					"N cpus to M codecs link is not supported yet\n");
+					"%s codec number %d < cpu number %d is not supported\n",
+					__func__, rtd->dai_link->num_codecs,
+					rtd->dai_link->num_cpus);
 				return -EINVAL;
 			}