diff --git a/drivers/soundwire/cadence_master.c b/drivers/soundwire/cadence_master.c index 61a8e12e1f3826..2917c19084f425 100644 --- a/drivers/soundwire/cadence_master.c +++ b/drivers/soundwire/cadence_master.c @@ -1345,13 +1345,14 @@ static u32 cdns_set_initial_frame_shape(int n_rows, int n_cols) return val; } -static void cdns_init_clock_ctrl(struct sdw_cdns *cdns) +static int cdns_init_clock_ctrl(struct sdw_cdns *cdns) { struct sdw_bus *bus = &cdns->bus; struct sdw_master_prop *prop = &bus->prop; u32 val; u32 ssp_interval; int divider; + int freq; dev_dbg(cdns->dev, "mclk %d max %d row %d col %d\n", prop->mclk_freq, @@ -1360,13 +1361,25 @@ static void cdns_init_clock_ctrl(struct sdw_cdns *cdns) prop->default_col); /* Set clock divider */ - divider = (prop->mclk_freq / prop->max_clk_freq) - 1; + divider = (prop->mclk_freq * SDW_DOUBLE_RATE_FACTOR / + bus->params.curr_dr_freq) - 1; + freq = bus->params.curr_dr_freq >> 1; cdns_updatel(cdns, CDNS_MCP_CLK_CTRL0, CDNS_MCP_CLK_MCLKD_MASK, divider); cdns_updatel(cdns, CDNS_MCP_CLK_CTRL1, CDNS_MCP_CLK_MCLKD_MASK, divider); + /* Set frame shape base on the actual bus frequency. */ + if (!prop->default_frame_rate || !prop->default_row) { + dev_err(cdns->dev, "Default frame_rate %d or row %d is invalid\n", + prop->default_frame_rate, prop->default_row); + return -EINVAL; + } + + prop->default_col = freq * SDW_DOUBLE_RATE_FACTOR / + prop->default_frame_rate / prop->default_row; + /* * Frame shape changes after initialization have to be done * with the bank switch mechanism @@ -1379,6 +1392,8 @@ static void cdns_init_clock_ctrl(struct sdw_cdns *cdns) ssp_interval = prop->default_frame_rate / SDW_CADENCE_GSYNC_HZ; cdns_writel(cdns, CDNS_MCP_SSP_CTRL0, ssp_interval); cdns_writel(cdns, CDNS_MCP_SSP_CTRL1, ssp_interval); + + return 0; } /** @@ -1412,9 +1427,12 @@ EXPORT_SYMBOL(sdw_cdns_soft_reset); */ int sdw_cdns_init(struct sdw_cdns *cdns) { + int ret; u32 val; - cdns_init_clock_ctrl(cdns); + ret = cdns_init_clock_ctrl(cdns); + if (ret) + return ret; sdw_cdns_check_self_clearing_bits(cdns, __func__, false, 0); diff --git a/drivers/soundwire/intel_auxdevice.c b/drivers/soundwire/intel_auxdevice.c index 599954d9275299..dee126f6d9d5a2 100644 --- a/drivers/soundwire/intel_auxdevice.c +++ b/drivers/soundwire/intel_auxdevice.c @@ -222,30 +222,9 @@ static int sdw_master_read_intel_prop(struct sdw_bus *bus) static int intel_prop_read(struct sdw_bus *bus) { - struct sdw_master_prop *prop; - /* Initialize with default handler to read all DisCo properties */ sdw_master_read_prop(bus); - /* - * Only one bus frequency is supported so far, filter - * frequencies reported in the DSDT - */ - prop = &bus->prop; - if (prop->clk_freq && prop->num_clk_freq > 1) { - unsigned int default_bus_frequency; - - default_bus_frequency = - prop->default_frame_rate * - prop->default_row * - prop->default_col / - SDW_DOUBLE_RATE_FACTOR; - - prop->num_clk_freq = 1; - prop->clk_freq[0] = default_bus_frequency; - prop->max_clk_freq = default_bus_frequency; - } - /* read Intel-specific properties */ sdw_master_read_intel_prop(bus);