1 files changed, 49 insertions, 22 deletions

diff --git a/sound/soc/codecs/rt1316-sdw.c b/sound/soc/codecs/rt1316-sdw.c
index ed0a11436362..e6294cc7a995 100644
--- a/sound/soc/codecs/rt1316-sdw.c
+++ b/sound/soc/codecs/rt1316-sdw.c
@@ -14,6 +14,7 @@
 #include <sound/core.h>
 #include <sound/pcm.h>
 #include <sound/pcm_params.h>
+#include <sound/sdw.h>
 #include <sound/soc-dapm.h>
 #include <sound/initval.h>
 #include "rt1316-sdw.h"
@@ -254,6 +255,17 @@ static int rt1316_read_prop(struct sdw_slave *slave)
 	return 0;
 }
 
+static void rt1316_apply_bq_params(struct rt1316_sdw_priv *rt1316)
+{
+	unsigned int i, reg, data;
+
+	for (i = 0; i < rt1316->bq_params_cnt; i += 3) {
+		reg = rt1316->bq_params[i] | (rt1316->bq_params[i + 1] << 8);
+		data = rt1316->bq_params[i + 2];
+		regmap_write(rt1316->regmap, reg, data);
+	}
+}
+
 static int rt1316_io_init(struct device *dev, struct sdw_slave *slave)
 {
 	struct rt1316_sdw_priv *rt1316 = dev_get_drvdata(dev);
@@ -519,11 +531,10 @@ static int rt1316_sdw_hw_params(struct snd_pcm_substream *substream,
 	struct snd_soc_component *component = dai->component;
 	struct rt1316_sdw_priv *rt1316 =
 		snd_soc_component_get_drvdata(component);
-	struct sdw_stream_config stream_config;
-	struct sdw_port_config port_config;
-	enum sdw_data_direction direction;
+	struct sdw_stream_config stream_config = {0};
+	struct sdw_port_config port_config = {0};
 	struct sdw_stream_data *stream;
-	int retval, port, num_channels, ch_mask;
+	int retval;
 
 	dev_dbg(dai->dev, "%s %s", __func__, dai->name);
 	stream = snd_soc_dai_get_dma_data(dai, substream);
@@ -535,25 +546,13 @@ static int rt1316_sdw_hw_params(struct snd_pcm_substream *substream,
 		return -EINVAL;
 
 	/* SoundWire specific configuration */
-	/* port 1 for playback */
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		direction = SDW_DATA_DIR_RX;
-		port = 1;
-	} else {
-		direction = SDW_DATA_DIR_TX;
-		port = 2;
-	}
-
-	num_channels = params_channels(params);
-	ch_mask = (1 << num_channels) - 1;
+	snd_sdw_params_to_config(substream, params, &stream_config, &port_config);
 
-	stream_config.frame_rate = params_rate(params);
-	stream_config.ch_count = num_channels;
-	stream_config.bps = snd_pcm_format_width(params_format(params));
-	stream_config.direction = direction;
-
-	port_config.ch_mask = ch_mask;
-	port_config.num = port;
+	/* port 1 for playback */
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		port_config.num = 1;
+	else
+		port_config.num = 2;
 
 	retval = sdw_stream_add_slave(rt1316->sdw_slave, &stream_config,
 				&port_config, 1, stream->sdw_stream);
@@ -590,14 +589,42 @@ static struct sdw_slave_ops rt1316_slave_ops = {
 	.update_status = rt1316_update_status,
 };
 
+static int rt1316_sdw_parse_dt(struct rt1316_sdw_priv *rt1316, struct device *dev)
+{
+	int ret = 0;
+
+	device_property_read_u32(dev, "realtek,bq-params-cnt", &rt1316->bq_params_cnt);
+	if (rt1316->bq_params_cnt) {
+		rt1316->bq_params = devm_kzalloc(dev, rt1316->bq_params_cnt, GFP_KERNEL);
+		if (!rt1316->bq_params) {
+			dev_err(dev, "Could not allocate bq_params memory\n");
+			ret = -ENOMEM;
+		} else {
+			ret = device_property_read_u8_array(dev, "realtek,bq-params", rt1316->bq_params, rt1316->bq_params_cnt);
+			if (ret < 0)
+				dev_err(dev, "Could not read list of realtek,bq-params\n");
+		}
+	}
+
+	dev_dbg(dev, "bq_params_cnt=%d\n", rt1316->bq_params_cnt);
+	return ret;
+}
+
 static int rt1316_sdw_component_probe(struct snd_soc_component *component)
 {
+	struct rt1316_sdw_priv *rt1316 = snd_soc_component_get_drvdata(component);
 	int ret;
 
+	rt1316->component = component;
+	rt1316_sdw_parse_dt(rt1316, &rt1316->sdw_slave->dev);
+
 	ret = pm_runtime_resume(component->dev);
 	if (ret < 0 && ret != -EACCES)
 		return ret;
 
+	/* apply BQ params */
+	rt1316_apply_bq_params(rt1316);
+
 	return 0;
 }