diff --git a/drivers/soundwire/bus.h b/drivers/soundwire/bus.h index 44e4f51939176f..476387b704723f 100644 --- a/drivers/soundwire/bus.h +++ b/drivers/soundwire/bus.h @@ -155,6 +155,8 @@ struct sdw_slave_runtime { * * @bus: Bus handle * @stream: Stream runtime handle + * @params_backup: Snapshot of original bus parameters saved before stream + * preparation, used for rollback recovery on failure. * @direction: Data direction for Master * @ch_count: Number of channels handled by the Master for * this stream, can be zero. @@ -166,6 +168,7 @@ struct sdw_slave_runtime { struct sdw_master_runtime { struct sdw_bus *bus; struct sdw_stream_runtime *stream; + struct sdw_bus_params params_backup; enum sdw_data_direction direction; unsigned int ch_count; struct list_head slave_rt_list; diff --git a/drivers/soundwire/generic_bandwidth_allocation.c b/drivers/soundwire/generic_bandwidth_allocation.c index 10ce5e4dee5a3a..29d9fcef8c709b 100644 --- a/drivers/soundwire/generic_bandwidth_allocation.c +++ b/drivers/soundwire/generic_bandwidth_allocation.c @@ -198,7 +198,7 @@ static void _sdw_compute_port_params(struct sdw_bus *bus, /* Run loop for all groups to compute transport parameters */ for (l = 0; l < SDW_MAX_LANES; l++) { - if (l > 0 && !bus->lane_used_bandwidth[l]) + if (l > 0 && !bus->params.lane_used_bandwidth[l]) continue; /* reset hstop for each lane */ hstop = bus->params.col - 1; @@ -269,7 +269,7 @@ static int sdw_compute_group_params(struct sdw_bus *bus, } for (l = 0; l < SDW_MAX_LANES; l++) { - if (l > 0 && !bus->lane_used_bandwidth[l]) + if (l > 0 && !bus->params.lane_used_bandwidth[l]) continue; /* reset column_needed for each lane */ column_needed = 0; @@ -509,7 +509,7 @@ static int get_manager_lane(struct sdw_bus *bus, struct sdw_master_runtime *m_rt m_rt->stream->params.bps; } if (required_bandwidth <= - curr_dr_freq - bus->lane_used_bandwidth[l]) { + curr_dr_freq - bus->params.lane_used_bandwidth[l]) { /* Check if m_lane is connected to all Peripherals */ if (!is_lane_connected_to_all_peripherals(m_rt, slave_prop->lane_maps[l])) { @@ -520,7 +520,7 @@ static int get_manager_lane(struct sdw_bus *bus, struct sdw_master_runtime *m_rt } m_lane = slave_prop->lane_maps[l]; dev_dbg(&s_rt->slave->dev, "M lane %d is used\n", m_lane); - bus->lane_used_bandwidth[l] += required_bandwidth; + bus->params.lane_used_bandwidth[l] += required_bandwidth; /* * Use non-zero manager lane, subtract the lane 0 * bandwidth that is already calculated diff --git a/drivers/soundwire/stream.c b/drivers/soundwire/stream.c index b4f68356708fe4..4a217b3b62c7bd 100644 --- a/drivers/soundwire/stream.c +++ b/drivers/soundwire/stream.c @@ -1487,19 +1487,22 @@ static int _sdw_prepare_stream(struct sdw_stream_runtime *stream, struct sdw_master_runtime *m_rt; struct sdw_bus *bus; struct sdw_master_prop *prop; - struct sdw_bus_params params; - int ret; + int ret, ret1; + + /* Pre-pass: snapshot original bus parameters for all masters */ + list_for_each_entry(m_rt, &stream->master_list, stream_node) + m_rt->params_backup = m_rt->bus->params; /* Prepare Master(s) and Slave(s) port(s) associated with stream */ list_for_each_entry(m_rt, &stream->master_list, stream_node) { bus = m_rt->bus; prop = &bus->prop; - memcpy(¶ms, &bus->params, sizeof(params)); /* TODO: Support Asynchronous mode */ if ((prop->max_clk_freq % stream->params.rate) != 0) { dev_err(bus->dev, "Async mode not supported\n"); - return -EINVAL; + ret = -EINVAL; + goto restore_params; } if (update_params) { @@ -1541,7 +1544,7 @@ static int _sdw_prepare_stream(struct sdw_stream_runtime *stream, if (ret < 0) { dev_err(bus->dev, "Prepare port(s) failed ret = %d\n", ret); - goto restore_params; + goto revert_bank; } } @@ -1549,8 +1552,36 @@ static int _sdw_prepare_stream(struct sdw_stream_runtime *stream, return ret; +revert_bank: + /* + * The bank values are already switched in do_bank_switch(), so we need to switch + * curr_bank and next_bank back to reflect the actual bank. + */ + list_for_each_entry(m_rt, &stream->master_list, stream_node) { + m_rt->params_backup.curr_bank = !m_rt->params_backup.curr_bank; + m_rt->params_backup.next_bank = !m_rt->params_backup.next_bank; + } + restore_params: - memcpy(&bus->params, ¶ms, sizeof(params)); + list_for_each_entry(m_rt, &stream->master_list, stream_node) { + /* + * Revert all masters to their snapshotted original parameters. + * Any master that was not modified will safely revert to its own original state. + */ + m_rt->bus->params = m_rt->params_backup; + ret1 = sdw_program_params(m_rt->bus, true); + if (ret1 < 0) { + dev_err(m_rt->bus->dev, + "Program params failed: %d after restoring params\n", + ret1); + /* No need to do bank switch and return the original error code */ + return ret; + } + } + ret1 = do_bank_switch(stream); + if (ret1 < 0) + pr_err("%s: do_bank_switch failed: %d after restoring params\n", __func__, ret1); + return ret; } @@ -1802,8 +1833,8 @@ static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream) 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; - if (!bus->lane_used_bandwidth[p_rt->lane]) + bus->params.lane_used_bandwidth[p_rt->lane] -= bandwidth; + if (!bus->params.lane_used_bandwidth[p_rt->lane]) p_rt->lane = 0; } /* TODO: Update this during Device-Device support */ diff --git a/include/linux/soundwire/sdw.h b/include/linux/soundwire/sdw.h index 1b03bfad2e6bbe..326db122bbe6ea 100644 --- a/include/linux/soundwire/sdw.h +++ b/include/linux/soundwire/sdw.h @@ -592,6 +592,7 @@ enum sdw_port_prep_ops { * @bandwidth: Current bandwidth * @col: Active columns * @row: Active rows + * @lane_used_bandwidth: how much bandwidth in bits per second is used by each lane * @s_data_mode: NORMAL, STATIC or PRBS mode for all Slave ports * @m_data_mode: NORMAL, STATIC or PRBS mode for all Master ports. The value * should be the same to detect transmission issues, but can be different to @@ -605,6 +606,7 @@ struct sdw_bus_params { unsigned int bandwidth; unsigned int col; unsigned int row; + unsigned int lane_used_bandwidth[SDW_MAX_LANES]; int s_data_mode; int m_data_mode; }; @@ -1030,7 +1032,6 @@ struct sdw_stream_runtime { * @multi_link: Store bus property that indicates if multi links * are supported. This flag is populated by drivers after reading * appropriate firmware (ACPI/DT). - * @lane_used_bandwidth: how much bandwidth in bits per second is used by each lane * @is_present: indicates whether any peripheral is present on the bus. */ struct sdw_bus { @@ -1067,7 +1068,6 @@ struct sdw_bus { struct dentry *debugfs; #endif bool multi_link; - unsigned int lane_used_bandwidth[SDW_MAX_LANES]; bool is_present; };