diff --git a/drivers/soundwire/amd_manager.c b/drivers/soundwire/amd_manager.c index 61df5cecdccc..a3316efdf8ac 100644 --- a/drivers/soundwire/amd_manager.c +++ b/drivers/soundwire/amd_manager.c @@ -467,12 +467,16 @@ static u32 amd_sdw_read_ping_status(struct sdw_bus *bus) static int amd_sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream) { + struct amd_sdw_manager *amd_manager = to_amd_sdw(bus); struct sdw_transport_data t_data = {0}; struct sdw_master_runtime *m_rt; struct sdw_port_runtime *p_rt; struct sdw_bus_params *b_params = &bus->params; int port_bo, hstart, hstop, sample_int; - unsigned int rate, bps; + unsigned int rate, bps, channels; + unsigned int stream_slot_size, max_slots; + static unsigned int next_offset[AMD_SDW_MAX_MANAGER_COUNT] = {1}; + unsigned int inst_id = amd_manager->instance; port_bo = 0; hstart = 1; @@ -483,11 +487,51 @@ static int amd_sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { rate = m_rt->stream->params.rate; bps = m_rt->stream->params.bps; + channels = m_rt->stream->params.ch_count; sample_int = (bus->params.curr_dr_freq / rate); + + /* Compute slots required for this stream dynamically */ + stream_slot_size = bps * channels; + list_for_each_entry(p_rt, &m_rt->port_list, port_node) { - port_bo = (p_rt->num * 64) + 1; - dev_dbg(bus->dev, "p_rt->num=%d hstart=%d hstop=%d port_bo=%d\n", - p_rt->num, hstart, hstop, port_bo); + if (p_rt->num >= amd_manager->max_ports) { + dev_err(bus->dev, "Port %d exceeds max ports %d\n", + p_rt->num, amd_manager->max_ports); + return -EINVAL; + } + + if (!amd_manager->port_offset_map[p_rt->num]) { + /* + * port block offset calculation for 6MHz bus clock frequency with + * different frame sizes 50 x 10 and 125 x 2 + */ + if (bus->params.curr_dr_freq == 12000000) { + max_slots = bus->params.row * (bus->params.col - 1); + if (next_offset[inst_id] + stream_slot_size <= + (max_slots - 1)) { + amd_manager->port_offset_map[p_rt->num] = + next_offset[inst_id]; + next_offset[inst_id] += stream_slot_size; + } else { + dev_err(bus->dev, + "No space for port %d\n", p_rt->num); + return -ENOMEM; + } + } else { + /* + * port block offset calculation for 12MHz bus clock + * frequency + */ + amd_manager->port_offset_map[p_rt->num] = + (p_rt->num * 64) + 1; + } + } + port_bo = amd_manager->port_offset_map[p_rt->num]; + dev_dbg(bus->dev, + "Port=%d hstart=%d hstop=%d port_bo=%d slots=%d max_ports=%d\n", + p_rt->num, hstart, hstop, port_bo, stream_slot_size, + amd_manager->max_ports); + sdw_fill_xport_params(&p_rt->transport_params, p_rt->num, false, SDW_BLK_GRP_CNT_1, sample_int, port_bo, port_bo >> 8, hstart, hstop, @@ -1079,6 +1123,11 @@ static int amd_sdw_manager_probe(struct platform_device *pdev) default: return -EINVAL; } + amd_manager->max_ports = amd_manager->num_dout_ports + amd_manager->num_din_ports; + amd_manager->port_offset_map = devm_kcalloc(dev, amd_manager->max_ports, + sizeof(int), GFP_KERNEL); + if (!amd_manager->port_offset_map) + return -ENOMEM; prop = &amd_manager->bus.prop; prop->mclk_freq = AMD_SDW_BUS_BASE_FREQ; diff --git a/include/linux/soundwire/sdw_amd.h b/include/linux/soundwire/sdw_amd.h index fe31773d5210..470360a2723c 100644 --- a/include/linux/soundwire/sdw_amd.h +++ b/include/linux/soundwire/sdw_amd.h @@ -66,8 +66,10 @@ struct sdw_amd_dai_runtime { * @status: peripheral devices status array * @num_din_ports: number of input ports * @num_dout_ports: number of output ports + * @max_ports: total number of input ports and output ports * @cols_index: Column index in frame shape * @rows_index: Rows index in frame shape + * @port_offset_map: dynamic array to map port block offset * @instance: SoundWire manager instance * @quirks: SoundWire manager quirks * @wake_en_mask: wake enable mask per SoundWire manager @@ -92,10 +94,12 @@ struct amd_sdw_manager { int num_din_ports; int num_dout_ports; + int max_ports; int cols_index; int rows_index; + int *port_offset_map; u32 instance; u32 quirks; u32 wake_en_mask;