diff --git a/drivers/soundwire/generic_bandwidth_allocation.c b/drivers/soundwire/generic_bandwidth_allocation.c index abf9b85daa52dd..3669378b4c305f 100644 --- a/drivers/soundwire/generic_bandwidth_allocation.c +++ b/drivers/soundwire/generic_bandwidth_allocation.c @@ -324,6 +324,30 @@ static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq) return -EINVAL; } +static bool is_all_peripherals_connected(struct sdw_master_runtime *m_rt, unsigned int lane) +{ + struct sdw_slave_prop *slave_prop; + struct sdw_slave_runtime *s_rt; + int i; + + list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { + slave_prop = &s_rt->slave->prop; + for (i = 1; i < SDW_MAX_LANES; i++) { + if (slave_prop->lane_maps[i] == lane) { + dev_dbg(&s_rt->slave->dev, + "lane_maps[%d] is connected to %d\n", + i, slave_prop->lane_maps[i]); + break; + } + } + if (i == SDW_MAX_LANES) { + dev_dbg(&s_rt->slave->dev, "%d is not connected\n", lane); + return false; + } + } + return true; +} + /** * sdw_compute_bus_params: Compute bus parameters * @@ -331,11 +355,19 @@ static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq) */ static int sdw_compute_bus_params(struct sdw_bus *bus) { - unsigned int curr_dr_freq = 0; struct sdw_master_prop *mstr_prop = &bus->prop; + struct sdw_slave_prop *slave_prop; + struct sdw_port_runtime *m_p_rt; + struct sdw_port_runtime *s_p_rt; + struct sdw_master_runtime *m_rt; + unsigned int required_bandwidth; + struct sdw_slave_runtime *s_rt; + unsigned int curr_dr_freq = 0; + bool use_multi_lane = false; int i, clk_values, ret; bool is_gear = false; u32 *clk_buf; + int m_lane; if (mstr_prop->num_clk_gears) { clk_values = mstr_prop->num_clk_gears; @@ -370,16 +402,89 @@ static int sdw_compute_bus_params(struct sdw_bus *bus) } if (i == clk_values) { - dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n", +multilane: + dev_dbg(bus->dev, "%s: could not find clock value for bandwidth %d, checking multi-lane\n", __func__, bus->params.bandwidth); - return -EINVAL; + + m_rt = list_last_entry(&bus->m_rt_list, struct sdw_master_runtime, bus_node); + s_rt = list_first_entry(&m_rt->slave_rt_list, struct sdw_slave_runtime, m_rt_node); + slave_prop = &s_rt->slave->prop; + + /* + * Find an available manager lane for the first Peripheral + * Testing the first Peripheral is enough because we can't use multi-lane + * if we can't find any available lane for the first Peripheral + */ + for (i = 1; i < SDW_MAX_LANES; i++) { + if (!slave_prop->lane_maps[i]) + continue; + + required_bandwidth = 0; + list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) { + required_bandwidth += m_rt->stream->params.rate * + hweight32(m_p_rt->ch_mask) * + m_rt->stream->params.bps; + } + if (required_bandwidth <= curr_dr_freq - bus->lane_used_bandwidth[i]) { + /* Check if m_lane is connected to all Peripherals */ + if (!is_all_peripherals_connected(m_rt, slave_prop->lane_maps[i])) { + dev_dbg(bus->dev, + "some Peripherals are not connected to %d\n", + slave_prop->lane_maps[i]); + continue; + } + m_lane = slave_prop->lane_maps[i]; + dev_dbg(&s_rt->slave->dev, + "M lane %d P lane %d can be used\n", + m_lane, i); + bus->lane_used_bandwidth[i] += required_bandwidth; + /* + * Use non-zero manager lane, subtract the lane 0 + * bandwidth that is already calculated + */ + bus->params.bandwidth -= required_bandwidth; + use_multi_lane = true; + break; + } + } + + if (!use_multi_lane) { + dev_err(bus->dev, "%s: could not find an available lane\n", __func__); + return -EINVAL; + } + + /* Set Peripheral lanes */ + list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { + slave_prop = &s_rt->slave->prop; + for (i = 1; i < SDW_MAX_LANES; i++) { + if (slave_prop->lane_maps[i] == m_lane) { + dev_dbg(&s_rt->slave->dev, "Set Peripheral lane = %d\n", i); + list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) { + s_p_rt->lane = i; + break; + } + } + } + } + /* Set Manager lanes */ + /* Assume there is only one Manager in multi-lane configurations. */ + list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) { + m_p_rt->lane = m_lane; + } } ret = sdw_select_row_col(bus, curr_dr_freq); if (ret < 0) { - dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n", - __func__, curr_dr_freq); - return -EINVAL; + if (use_multi_lane) { + dev_err(bus->dev, + "%s: could not find frame configuration for bus dr_freq %d\n", + __func__, curr_dr_freq); + return -EINVAL; + } + dev_warn(bus->dev, + "%s: could not find frame configuration for bus dr_freq %d try multi-lane\n", + __func__, curr_dr_freq); + goto multilane; } bus->params.curr_dr_freq = curr_dr_freq; diff --git a/drivers/soundwire/stream.c b/drivers/soundwire/stream.c index 7aa4900dcf3172..bc9c6208673f0d 100644 --- a/drivers/soundwire/stream.c +++ b/drivers/soundwire/stream.c @@ -1641,7 +1641,11 @@ EXPORT_SYMBOL(sdw_disable_stream); static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream) { + unsigned int multi_lane_bandwidth; struct sdw_master_runtime *m_rt; + struct sdw_port_runtime *p_rt; + bool compute_params = true; + unsigned int bandwidth; struct sdw_bus *bus; int ret = 0; @@ -1655,12 +1659,27 @@ static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream) return ret; } + multi_lane_bandwidth = 0; + + list_for_each_entry(p_rt, &m_rt->port_list, port_node) { + if (!p_rt->lane) + continue; + + bandwidth = m_rt->stream->params.rate * hweight32(p_rt->ch_mask) * + m_rt->stream->params.bps; + multi_lane_bandwidth += bandwidth; + bus->lane_used_bandwidth[p_rt->lane] -= bandwidth; + } /* TODO: Update this during Device-Device support */ - bus->params.bandwidth -= m_rt->stream->params.rate * - m_rt->ch_count * m_rt->stream->params.bps; + bandwidth = m_rt->stream->params.rate * m_rt->ch_count * m_rt->stream->params.bps; + bus->params.bandwidth -= bandwidth - multi_lane_bandwidth; + + /* No need to compute params since bus->params.bandwidth is not changed */ + if (multi_lane_bandwidth == bandwidth) + compute_params = false; /* Compute params */ - if (bus->compute_params) { + if (compute_params && bus->compute_params) { ret = bus->compute_params(bus); if (ret < 0) { dev_err(bus->dev, "Compute params failed: %d\n",