diff --git a/drivers/soundwire/qcom.c b/drivers/soundwire/qcom.c index 03f20a929e4ba..063311cfd2a5b 100644 --- a/drivers/soundwire/qcom.c +++ b/drivers/soundwire/qcom.c @@ -163,6 +163,10 @@ struct qcom_swrm_port_config { u8 word_length; u8 blk_group_count; u8 lane_control; + u8 tx_ch_mask; + u8 rx_ch_mask; + bool tx_ch_map_valid; + bool rx_ch_map_valid; }; /* @@ -1143,12 +1147,28 @@ static int qcom_swrm_port_enable(struct sdw_bus *bus, { u32 reg; struct qcom_swrm_ctrl *ctrl = to_qcom_sdw(bus); + struct qcom_swrm_port_config *pcfg; u32 val; u32 offset = ctrl->reg_layout[SWRM_OFFSET_DP_PORT_CTRL_BANK]; reg = SWRM_DPn_PORT_CTRL_BANK(offset, enable_ch->port_num, bank); ctrl->reg_read(ctrl, reg, &val); + pcfg = &ctrl->pconfig[enable_ch->port_num]; + + mutex_lock(&ctrl->port_lock); + if (enable_ch->port_num <= ctrl->num_dout_ports) { + if (pcfg->rx_ch_map_valid) + enable_ch->ch_mask = pcfg->rx_ch_mask; + else if (pcfg->tx_ch_map_valid) + enable_ch->ch_mask = pcfg->tx_ch_mask; + } else { + if (pcfg->tx_ch_map_valid) + enable_ch->ch_mask = pcfg->tx_ch_mask; + else if (pcfg->rx_ch_map_valid) + enable_ch->ch_mask = pcfg->rx_ch_mask; + } + mutex_unlock(&ctrl->port_lock); if (enable_ch->enable) val |= (enable_ch->ch_mask << SWRM_DP_PORT_CTRL_EN_CHAN_SHFT); @@ -1368,6 +1388,39 @@ static void *qcom_swrm_get_sdw_stream(struct snd_soc_dai *dai, int direction) return ctrl->sruntime[dai->id]; } +static int qcom_swrm_set_channel_map(struct snd_soc_dai *dai, + unsigned int tx_num, const unsigned int *tx_slot, + unsigned int rx_num, const unsigned int *rx_slot) +{ + struct qcom_swrm_ctrl *ctrl = dev_get_drvdata(dai->dev); + unsigned int max_ports = ctrl->nports; + unsigned int i; + + if ((tx_num && !tx_slot) || (rx_num && !rx_slot)) + return -EINVAL; + + mutex_lock(&ctrl->port_lock); + for (i = 1; i <= max_ports; i++) { + ctrl->pconfig[i].tx_ch_map_valid = false; + ctrl->pconfig[i].rx_ch_map_valid = false; + + /* TX setup */ + if (tx_slot && i < tx_num) { + ctrl->pconfig[i].tx_ch_mask = tx_slot[i]; + ctrl->pconfig[i].tx_ch_map_valid = true; + } + + /* RX setup */ + if (rx_slot && i < rx_num) { + ctrl->pconfig[i].rx_ch_mask = rx_slot[i]; + ctrl->pconfig[i].rx_ch_map_valid = true; + } + } + mutex_unlock(&ctrl->port_lock); + + return 0; +} + static int qcom_swrm_startup(struct snd_pcm_substream *substream, struct snd_soc_dai *dai) { @@ -1404,6 +1457,7 @@ static const struct snd_soc_dai_ops qcom_swrm_pdm_dai_ops = { .shutdown = qcom_swrm_shutdown, .set_stream = qcom_swrm_set_sdw_stream, .get_stream = qcom_swrm_get_sdw_stream, + .set_channel_map = qcom_swrm_set_channel_map, }; static const struct snd_soc_component_driver qcom_swrm_dai_component = {