Merge "swr-wcd-ctrl: Ensure soundwire banks are always in sync" (1336761f) · Commits · e / devices / android_kernel_xiaomi_markw

drivers/soundwire/soundwire.c

+32 −0

Original line number	Diff line number	Diff line
		@@ -239,6 +239,38 @@ void swr_port_response(struct swr_master *mstr, u8 tid)
		}
		EXPORT_SYMBOL(swr_port_response);

		/**
		* swr_remove_from_group - remove soundwire slave devices from group
		* @dev: pointer to the soundwire slave device
		* dev_num: device number of the soundwire slave device
		*
		* Returns error code for failure and 0 for success
		*/
		int swr_remove_from_group(struct swr_device *dev, u8 dev_num)
		{
		struct swr_master *master;

		if (!dev)
		return -ENODEV;

		master = dev->master;
		if (!master)
		return -EINVAL;

		if (!dev->group_id)
		return 0;

		if (master->gr_sid == dev_num)
		return 0;

		if (master->remove_from_group && master->remove_from_group(master))
		dev_dbg(&master->dev, "%s: falling back to GROUP_NONE\n",
		__func__);

		return 0;
		}
		EXPORT_SYMBOL(swr_remove_from_group);

		/**
		* swr_slvdev_datapath_control - Enables/Disables soundwire slave device
		* data path

drivers/soundwire/swr-wcd-ctrl.c

+215 −70

Original line number	Diff line number	Diff line
		@@ -167,6 +167,8 @@ enum {
		#define SWR_MSTR_RD_BUF_LEN 8
		#define SWR_MSTR_WR_BUF_LEN 32

		static void swrm_copy_data_port_config(struct swr_master *master,
		u8 inactive_bank);
		static struct swr_mstr_ctrl *dbgswrm;
		static struct dentry *debugfs_swrm_dent;
		static struct dentry *debugfs_peek;
		@@ -591,6 +593,42 @@ static struct swr_port_info swrm_get_port(struct swr_master master,
		int i;
		struct swr_port_info *port = NULL;

		for (i = 0; i < SWR_MSTR_PORT_LEN; i++) {
		port = &master->port[i];
		if (port->port_id == port_id) {
		dev_dbg(&master->dev, "%s: port_id: %d, index: %d\n",
		__func__, port_id, i);
		return port;
		}
		}

		return NULL;
		}

		static struct swr_port_info swrm_get_avail_port(struct swr_master master)
		{
		int i;
		struct swr_port_info *port = NULL;

		for (i = 0; i < SWR_MSTR_PORT_LEN; i++) {
		port = &master->port[i];
		if (port->port_en)
		continue;

		dev_dbg(&master->dev, "%s: port_id: %d, index: %d\n",
		__func__, port->port_id, i);
		return port;
		}

		return NULL;
		}

		static struct swr_port_info swrm_get_enabled_port(struct swr_master master,
		u8 port_id)
		{
		int i;
		struct swr_port_info *port = NULL;

		for (i = 0; i < SWR_MSTR_PORT_LEN; i++) {
		port = &master->port[i];
		if ((port->port_id == port_id) && (port->port_en == true))
		@@ -601,6 +639,96 @@ static struct swr_port_info swrm_get_port(struct swr_master master,
		return port;
		}

		static bool swrm_remove_from_group(struct swr_master *master)
		{
		struct swr_device *swr_dev;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);
		bool is_removed = false;

		if (!swrm)
		goto end;

		mutex_lock(&swrm->mlock);
		if ((swrm->num_rx_chs > 1) &&
		(swrm->num_rx_chs == swrm->num_cfg_devs)) {
		list_for_each_entry(swr_dev, &master->devices,
		dev_list) {
		swr_dev->group_id = SWR_GROUP_NONE;
		master->gr_sid = 0;
		}
		is_removed = true;
		}
		mutex_unlock(&swrm->mlock);

		end:
		return is_removed;
		}

		static void swrm_cleanup_disabled_data_ports(struct swr_master *master,
		u8 bank)
		{
		u32 value;
		struct swr_port_info *port;
		int i;
		int port_type;
		struct swrm_mports mport, mport_next = NULL;
		int port_disable_cnt = 0;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);

		dev_dbg(swrm->dev, "%s: master num_port: %d\n", __func__,
		master->num_port);

		mport = list_first_entry_or_null(&swrm->mport_list,
		struct swrm_mports,
		list);
		if (!mport) {
		dev_err(swrm->dev, "%s: list is empty\n", __func__);
		return;
		}

		for (i = 0; i < master->num_port; i++) {
		port = swrm_get_port(master, mstr_ports[mport->id]);
		if (!port \|\| port->ch_en)
		goto inc_loop;

		port_disable_cnt++;
		port_type = mstr_port_type[mport->id];
		value = ((port->ch_en)
		<< SWRM_DP_PORT_CTRL_EN_CHAN_SHFT);
		value \|= ((port->offset2)
		<< SWRM_DP_PORT_CTRL_OFFSET2_SHFT);
		value \|= ((port->offset1)
		<< SWRM_DP_PORT_CTRL_OFFSET1_SHFT);
		value \|= port->sinterval;

		swrm->write(swrm->handle,
		SWRM_DP_PORT_CTRL_BANK((mport->id+1), bank),
		value);
		swrm_cmd_fifo_wr_cmd(swrm, 0x00, port->dev_id, 0x00,
		SWRS_DP_CHANNEL_ENABLE_BANK(port_type, bank));

		dev_dbg(swrm->dev, "%s: mport :%d, reg: 0x%x, val: 0x%x\n",
		__func__, mport->id,
		(SWRM_DP_PORT_CTRL_BANK((mport->id+1), bank)), value);

		inc_loop:
		mport_next = list_next_entry(mport, list);
		if (port && !port->ch_en) {
		list_del(&mport->list);
		kfree(mport);
		}
		if (!mport_next) {
		dev_err(swrm->dev, "%s: end of list\n", __func__);
		break;
		}
		mport = mport_next;
		}
		master->num_port -= port_disable_cnt;

		dev_dbg(swrm->dev, "%s:disable ports: %d, active ports (rem): %d\n",
		__func__, port_disable_cnt, master->num_port);
		}

		static void swrm_slvdev_datapath_control(struct swr_master *master,
		bool enable)
		{
		@@ -610,24 +738,14 @@ static void swrm_slvdev_datapath_control(struct swr_master *master,
		int mask = (SWRM_MCP_FRAME_CTRL_BANK_ROW_CTRL_BMSK \|
		SWRM_MCP_FRAME_CTRL_BANK_COL_CTRL_BMSK \|
		SWRM_MCP_FRAME_CTRL_BANK_SSP_PERIOD_BMSK);
		int col_mask = SWRM_MCP_FRAME_CTRL_BANK_COL_CTRL_BMSK;
		u8 active_bank;
		u8 inactive_bank;

		bank = get_inactive_bank_num(swrm);

		dev_dbg(swrm->dev, "%s: enable: %d, slvdev_dp_enable_cnt: %d\n",
		__func__, enable, swrm->slvdev_dp_enable_cnt);
		dev_dbg(swrm->dev, "%s: enable: %d, cfg_devs: %d\n",
		__func__, enable, swrm->num_cfg_devs);

		if (enable) {
		swrm->slvdev_dp_enable_cnt++;
		active_bank = bank ? 0 : 1;
		value = swrm->read(swrm->handle,
		SWRM_MCP_FRAME_CTRL_BANK_ADDR(active_bank));
		if (((value & col_mask) >>
		SWRM_MCP_FRAME_CTRL_BANK_COL_CTRL_SHFT) ==
		SWR_MAX_COL)
		return;

		/* set Row = 48 and col = 16 */
		n_col = SWR_MAX_COL;
		} else {
		@@ -636,12 +754,19 @@ static void swrm_slvdev_datapath_control(struct swr_master *master,
		* as stereo and if disable datapath is called for the
		* first slave device
		*/
		swrm->slvdev_dp_enable_cnt--;
		if (swrm->slvdev_dp_enable_cnt > 0)
		return;

		/* set Row = 48 and col = 2 */
		if (swrm->num_cfg_devs > 0)
		n_col = SWR_MAX_COL;
		else
		n_col = SWR_MIN_COL;

		/*
		* All ports are already disabled, no need to perform
		* bank-switch and copy operation. This case can arise
		* when speaker channels are enabled in stereo mode with
		* BROADCAST and disabled in GROUP_NONE
		*/
		if (master->num_port == 0)
		return;
		}

		value = swrm->read(swrm->handle, SWRM_MCP_FRAME_CTRL_BANK_ADDR(bank));
		@@ -655,22 +780,26 @@ static void swrm_slvdev_datapath_control(struct swr_master *master,
		SWRM_MCP_FRAME_CTRL_BANK_ADDR(bank), value);

		enable_bank_switch(swrm, bank, SWR_MAX_ROW, n_col);

		inactive_bank = bank ? 0 : 1;
		if (enable)
		swrm_copy_data_port_config(master, inactive_bank);
		else
		swrm_cleanup_disabled_data_ports(master, inactive_bank);

		if (!swrm_is_port_en(master)) {
		dev_dbg(&master->dev, "%s: pm_runtime auto suspend triggered\n",
		__func__);
		pm_runtime_mark_last_busy(&swrm->pdev->dev);
		pm_runtime_put_autosuspend(&swrm->pdev->dev);
		}
		}

		static void swrm_apply_port_config(struct swr_master *master)
		{
		u32 value;
		struct swr_port_info *port;
		u8 bank;
		int i;
		int port_type;
		struct swrm_mports *mport;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);

		u32 reg[SWRM_MAX_PORT_REG];
		u32 val[SWRM_MAX_PORT_REG];
		int len = 0;

		if (!swrm) {
		pr_err("%s: Invalid handle to swr controller\n",
		__func__);
		@@ -685,6 +814,24 @@ static void swrm_apply_port_config(struct swr_master *master)
		swrm_cmd_fifo_wr_cmd(swrm, 0x01, 0xF, 0x00,
		SWRS_SCP_HOST_CLK_DIV2_CTL_BANK(bank));

		swrm_copy_data_port_config(master, bank);
		}

		static void swrm_copy_data_port_config(struct swr_master *master, u8 bank)
		{
		u32 value;
		struct swr_port_info *port;
		int i;
		int port_type;
		struct swrm_mports *mport;
		u32 reg[SWRM_MAX_PORT_REG];
		u32 val[SWRM_MAX_PORT_REG];
		int len = 0;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);

		dev_dbg(swrm->dev, "%s: master num_port: %d\n", __func__,
		master->num_port);

		mport = list_first_entry_or_null(&swrm->mport_list,
		struct swrm_mports,
		list);
		@@ -694,7 +841,7 @@ static void swrm_apply_port_config(struct swr_master *master)
		}
		for (i = 0; i < master->num_port; i++) {

		port = swrm_get_port(master, mstr_ports[mport->id]);
		port = swrm_get_enabled_port(master, mstr_ports[mport->id]);
		if (!port)
		continue;
		port_type = mstr_port_type[mport->id];
		@@ -756,6 +903,7 @@ static int swrm_connect_port(struct swr_master *master,
		int ret = 0;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);
		struct swrm_mports *mport;
		struct list_head ptr, next;

		dev_dbg(&master->dev, "%s: enter\n", __func__);
		if (!portinfo)
		@@ -786,8 +934,13 @@ static int swrm_connect_port(struct swr_master *master,
		__func__, portinfo->port_id[i]);
		goto port_fail;
		}
		port = swrm_get_avail_port(master);
		if (!port) {
		dev_err(&master->dev,
		"%s: avail ports not found!\n", __func__);
		goto port_fail;
		}
		list_add(&mport->list, &swrm->mport_list);
		port = &master->port[(master->num_port+i)];
		port->dev_id = portinfo->dev_id;
		port->port_id = portinfo->port_id[i];
		port->num_ch = portinfo->num_ch[i];
		@@ -805,8 +958,10 @@ static int swrm_connect_port(struct swr_master *master,

		swrm_get_port_config(master);
		swr_port_response(master, portinfo->tid);
		if (swrm->num_rx_chs > 1) {
		swrm->num_cfg_devs += 1;
		dev_dbg(&master->dev, "%s: cfg_devs: %d, rx_chs: %d\n",
		__func__, swrm->num_cfg_devs, swrm->num_rx_chs);
		if (swrm->num_rx_chs > 1) {
		if (swrm->num_rx_chs == swrm->num_cfg_devs)
		swrm_apply_port_config(master);
		} else {
		@@ -818,6 +973,21 @@ static int swrm_connect_port(struct swr_master *master,
		port_fail:
		kfree(mport);
		mem_fail:
		list_for_each_safe(ptr, next, &swrm->mport_list) {
		mport = list_entry(ptr, struct swrm_mports, list);
		if (!mport)
		continue;
		for (i = 0; i < portinfo->num_port; i++) {
		if (portinfo->port_id[i] == mstr_ports[mport->id]) {
		port = swrm_get_port(master,
		portinfo->port_id[i]);
		port->ch_en = false;
		list_del(&mport->list);
		kfree(mport);
		break;
		}
		}
		}
		mutex_unlock(&swrm->mlock);
		return ret;
		}
		@@ -827,16 +997,12 @@ static int swrm_disconnect_port(struct swr_master *master,
		{
		int i;
		struct swr_port_info *port;
		struct swrm_mports *mport;
		struct list_head ptr, next;
		u8 bank, active_bank;
		u8 bank;
		u32 value;
		int ret = 0;
		u8 mport_id = 0;
		int port_type = 0;
		struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(master);
		u32 reg[SWRM_MAX_PORT_REG];
		u32 val[SWRM_MAX_PORT_REG];
		int len = 0;

		if (!swrm) {
		dev_err(&master->dev,
		@@ -851,7 +1017,6 @@ static int swrm_disconnect_port(struct swr_master *master,
		}
		mutex_lock(&swrm->mlock);
		bank = get_inactive_bank_num(swrm);
		active_bank = bank ? 0 : 1;
		for (i = 0; i < portinfo->num_port; i++) {
		ret = swrm_get_master_port(&mport_id,
		portinfo->port_id[i]);
		@@ -862,7 +1027,7 @@ static int swrm_disconnect_port(struct swr_master *master,
		mutex_unlock(&swrm->mlock);
		return -EINVAL;
		}
		port = swrm_get_port(master, portinfo->port_id[i]);
		port = swrm_get_enabled_port(master, portinfo->port_id[i]);
		if (!port) {
		dev_dbg(&master->dev, "%s: port %d already disabled\n",
		__func__, portinfo->port_id[i]);
		@@ -872,46 +1037,26 @@ static int swrm_disconnect_port(struct swr_master *master,
		port->dev_id = portinfo->dev_id;
		port->port_en = false;
		port->ch_en = 0;
		value = port->ch_en << SWRM_DP_PORT_CTRL_EN_CHAN_SHFT;
		value \|= (port->offset2 << SWRM_DP_PORT_CTRL_OFFSET2_SHFT);
		value \|= (port->offset1 << SWRM_DP_PORT_CTRL_OFFSET1_SHFT);
		value \|= port->sinterval;


		swrm->write(swrm->handle,
		SWRM_DP_PORT_CTRL_BANK((mport_id+1), bank),
		0);
		value);
		swrm_cmd_fifo_wr_cmd(swrm, 0x00, port->dev_id, 0x00,
		SWRS_DP_CHANNEL_ENABLE_BANK(port_type, bank));
		reg[len] = SWRM_DP_PORT_CTRL_BANK((mport_id+1), active_bank);
		val[len++] = 0;
		reg[len] = SWRM_CMD_FIFO_WR_CMD;
		val[len++] = SWR_REG_VAL_PACK(0x00, port->dev_id, 0x00,
		SWRS_DP_CHANNEL_ENABLE_BANK(port_type,
		active_bank));
		list_for_each_safe(ptr, next, &swrm->mport_list) {
		mport = list_entry(ptr, struct swrm_mports, list);
		if (mport->id == mport_id) {
		list_del(&mport->list);
		kfree(mport);
		}
		}
		}
		enable_bank_switch(swrm, bank, SWR_MAX_ROW, SWR_MAX_COL);
		swrm->bulk_write(swrm->handle, reg, val, len);
		if (master->num_port >= SWR_MSTR_PORT_LEN)
		master->num_port = SWR_MSTR_PORT_LEN;

		master->num_port -= portinfo->num_port;
		swr_port_response(master, portinfo->tid);
		if (swrm->num_rx_chs > 1)
		swrm->num_cfg_devs -= 1;
		dev_dbg(&master->dev, "%s: cfg_devs: %d, rx_chs: %d, active ports: %d\n",
		__func__, swrm->num_cfg_devs, swrm->num_rx_chs,
		master->num_port);
		mutex_unlock(&swrm->mlock);

		dev_dbg(&master->dev, "%s: master active ports: %d\n",
		__func__, master->num_port);

		if (!swrm_is_port_en(master)) {
		dev_dbg(&master->dev, "%s: pm_runtime auto suspend triggered\n",
		__func__);
		pm_runtime_mark_last_busy(&swrm->pdev->dev);
		pm_runtime_put_autosuspend(&swrm->pdev->dev);
		}
		return 0;
		}

		@@ -1099,7 +1244,6 @@ static int swrm_master_init(struct swr_mstr_ctrl *swrm)
		u32 value[SWRM_MAX_INIT_REG];
		int len = 0;

		swrm->slvdev_dp_enable_cnt = 0;
		/* Clear Rows and Cols */
		val = ((row_ctrl << SWRM_MCP_FRAME_CTRL_BANK_ROW_CTRL_SHFT) \|
		(col_ctrl << SWRM_MCP_FRAME_CTRL_BANK_COL_CTRL_SHFT) \|
		@@ -1218,6 +1362,7 @@ static int swrm_probe(struct platform_device *pdev)
		swrm->master.connect_port = swrm_connect_port;
		swrm->master.disconnect_port = swrm_disconnect_port;
		swrm->master.slvdev_datapath_control = swrm_slvdev_datapath_control;
		swrm->master.remove_from_group = swrm_remove_from_group;
		swrm->master.dev.parent = &pdev->dev;
		swrm->master.dev.of_node = pdev->dev.of_node;
		swrm->master.num_port = 0;
		@@ -1524,7 +1669,7 @@ int swrm_wcd_notify(struct platform_device pdev, u32 id, void data)
		} else {
		mutex_lock(&swrm->mlock);
		swrm->num_rx_chs = (int )data;
		if (swrm->num_rx_chs > 1) {
		if ((swrm->num_rx_chs > 1) && !swrm->num_cfg_devs) {
		list_for_each_entry(swr_dev, &mstr->devices,
		dev_list) {
		ret = swr_set_device_group(swr_dev,

drivers/soundwire/swr-wcd-ctrl.h

+0 −1

Original line number	Diff line number	Diff line
		@@ -96,7 +96,6 @@ struct swr_mstr_ctrl {
		struct platform_device *pdev;
		int num_rx_chs;
		u8 num_cfg_devs;
		u8 slvdev_dp_enable_cnt;
		};

		#endif /* _SWR_WCD_CTRL_H */

include/linux/soundwire/soundwire.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -152,6 +152,7 @@ struct swr_master {
		int (get_logical_dev_num)(struct swr_master mstr, u64 dev_id,
		u8 *dev_num);
		void (slvdev_datapath_control)(struct swr_master mstr, bool enable);
		bool (remove_from_group)(struct swr_master mstr);
		};

		static inline struct swr_master to_swr_master(struct device dev)
		@@ -307,4 +308,5 @@ extern int swr_reset_device(struct swr_device *swr_dev);

		extern int swr_slvdev_datapath_control(struct swr_device *swr_dev, u8 dev_num,
		bool enable);
		extern int swr_remove_from_group(struct swr_device *dev, u8 dev_num);
		#endif /* _LINUX_SOUNDWIRE_H */

sound/soc/codecs/wsa881x.c

+3 −0

Original line number	Diff line number	Diff line
		@@ -853,6 +853,9 @@ static int wsa881x_spkr_pa_event(struct snd_soc_dapm_widget *w,
		}
		schedule_delayed_work(&wsa881x->ocp_ctl_work,
		msecs_to_jiffies(WSA881X_OCP_CTL_TIMER_SEC * 1000));
		/* Force remove group */
		swr_remove_from_group(wsa881x->swr_slave,
		wsa881x->swr_slave->dev_num);
		break;
		case SND_SOC_DAPM_POST_PMD:
		if (wsa881x->visense_enable) {