diff --git a/drivers/soundwire/swr-wcd-ctrl.c b/drivers/soundwire/swr-wcd-ctrl.c index e4424359c692..3a43585f707e 100644 --- a/drivers/soundwire/swr-wcd-ctrl.c +++ b/drivers/soundwire/swr-wcd-ctrl.c @@ -761,7 +761,13 @@ static int swrm_connect_port(struct swr_master *master, swrm_get_port_config(master); swr_port_response(master, portinfo->tid); - swrm_apply_port_config(master); + if (swrm->num_rx_chs > 1) { + swrm->num_cfg_devs += 1; + if (swrm->num_rx_chs == swrm->num_cfg_devs) + swrm_apply_port_config(master); + } else { + swrm_apply_port_config(master); + } mutex_unlock(&swrm->mlock); return 0; @@ -849,6 +855,8 @@ static int swrm_disconnect_port(struct swr_master *master, master->num_port -= portinfo->num_port; swr_port_response(master, portinfo->tid); + if (swrm->num_rx_chs > 1) + swrm->num_cfg_devs -= 1; mutex_unlock(&swrm->mlock); dev_dbg(&master->dev, "%s: master active ports: %d\n", @@ -1168,6 +1176,7 @@ static int swrm_probe(struct platform_device *pdev) swrm->rcmd_id = 0; swrm->wcmd_id = 0; swrm->slave_status = 0; + swrm->num_rx_chs = 0; swrm->state = SWR_MSTR_RESUME; init_completion(&swrm->reset); init_completion(&swrm->broadcast); @@ -1453,6 +1462,27 @@ int swrm_wcd_notify(struct platform_device *pdev, u32 id, void *data) } mutex_unlock(&swrm->mlock); break; + case SWR_SET_NUM_RX_CH: + if (!data) { + dev_err(swrm->dev, "%s: data is NULL\n", __func__); + ret = -EINVAL; + } else { + mutex_lock(&swrm->mlock); + swrm->num_rx_chs = *(int *)data; + if (swrm->num_rx_chs > 1) { + list_for_each_entry(swr_dev, &mstr->devices, + dev_list) { + ret = swr_set_device_group(swr_dev, + SWR_BROADCAST); + if (ret) + dev_err(swrm->dev, + "%s: set num ch failed\n", + __func__); + } + } + mutex_unlock(&swrm->mlock); + } + break; default: dev_err(swrm->dev, "%s: swr master unknown id %d\n", __func__, id); diff --git a/drivers/soundwire/swr-wcd-ctrl.h b/drivers/soundwire/swr-wcd-ctrl.h index 8fe17c72b410..b5d477523315 100755 --- a/drivers/soundwire/swr-wcd-ctrl.h +++ b/drivers/soundwire/swr-wcd-ctrl.h @@ -94,6 +94,8 @@ struct swr_mstr_ctrl { struct list_head mport_list; int state; struct platform_device *pdev; + int num_rx_chs; + u8 num_cfg_devs; }; #endif /* _SWR_WCD_CTRL_H */ diff --git a/include/linux/soundwire/swr-wcd.h b/include/linux/soundwire/swr-wcd.h index 0c09d5e19a0d..041b901fd954 100755 --- a/include/linux/soundwire/swr-wcd.h +++ b/include/linux/soundwire/swr-wcd.h @@ -21,7 +21,8 @@ enum { SWR_CH_MAP, SWR_DEVICE_DOWN, SWR_DEVICE_UP, - SWR_SUBSYS_RESTART + SWR_SUBSYS_RESTART, + SWR_SET_NUM_RX_CH, }; struct swr_mstr_port {