From a78b27707231d1eb6e55a8c0af42307021852102 Mon Sep 17 00:00:00 2001 From: Phani Kumar Uppalapati Date: Sat, 30 Apr 2016 12:16:00 -0700 Subject: [PATCH] swr-wcd-ctrl: Ensure soundwire banks are always in sync Copy speaker configuration from active to inactive bank and perform bank switch operation while speaker channels are getting enabled or disabled. This will make sure that soundwire banks are always in sync and allow independent control of speaker channels. CRs-fixed: 1007465 Change-Id: Ic1653194c22fa5669b1c04fd9630158633fb00a5 Signed-off-by: Phani Kumar Uppalapati --- drivers/soundwire/swr-wcd-ctrl.c | 252 +++++++++++++++++++++++-------- drivers/soundwire/swr-wcd-ctrl.h | 1 - 2 files changed, 186 insertions(+), 67 deletions(-) diff --git a/drivers/soundwire/swr-wcd-ctrl.c b/drivers/soundwire/swr-wcd-ctrl.c index 9ab3d7cf37b0..d861326b6f7f 100644 --- a/drivers/soundwire/swr-wcd-ctrl.c +++ b/drivers/soundwire/swr-wcd-ctrl.c @@ -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)) @@ -626,6 +664,71 @@ 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) { @@ -635,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 { @@ -661,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; + if (swrm->num_cfg_devs > 0) + n_col = SWR_MAX_COL; + else + n_col = SWR_MIN_COL; - /* set Row = 48 and col = 2 */ - 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)); @@ -680,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__); @@ -710,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); @@ -719,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]; @@ -781,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) @@ -811,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]; @@ -831,6 +959,8 @@ static int swrm_connect_port(struct swr_master *master, swrm_get_port_config(master); swr_port_response(master, portinfo->tid); 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); @@ -843,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; } @@ -852,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, @@ -876,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]); @@ -887,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]); @@ -897,45 +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); 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; } @@ -1123,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) | diff --git a/drivers/soundwire/swr-wcd-ctrl.h b/drivers/soundwire/swr-wcd-ctrl.h index a9248425deea..8992318cdbd3 100644 --- a/drivers/soundwire/swr-wcd-ctrl.h +++ b/drivers/soundwire/swr-wcd-ctrl.h @@ -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 */