static int ti_opp_supply_set_opp()

in ti-opp-supply.c [276:352]

• 61 lines of code
• 13 McCabe index (conditional complexity)

static int ti_opp_supply_set_opp(struct dev_pm_set_opp_data *data)
{
	struct dev_pm_opp_supply *old_supply_vdd = &data->old_opp.supplies[0];
	struct dev_pm_opp_supply *old_supply_vbb = &data->old_opp.supplies[1];
	struct dev_pm_opp_supply *new_supply_vdd = &data->new_opp.supplies[0];
	struct dev_pm_opp_supply *new_supply_vbb = &data->new_opp.supplies[1];
	struct device *dev = data->dev;
	unsigned long old_freq = data->old_opp.rate, freq = data->new_opp.rate;
	struct clk *clk = data->clk;
	struct regulator *vdd_reg = data->regulators[0];
	struct regulator *vbb_reg = data->regulators[1];
	int vdd_uv;
	int ret;

	vdd_uv = _get_optimal_vdd_voltage(dev, &opp_data,
					  new_supply_vdd->u_volt);

	if (new_supply_vdd->u_volt_min < vdd_uv)
		new_supply_vdd->u_volt_min = vdd_uv;

	/* Scaling up? Scale voltage before frequency */
	if (freq > old_freq) {
		ret = _opp_set_voltage(dev, new_supply_vdd, vdd_uv, vdd_reg,
				       "vdd");
		if (ret)
			goto restore_voltage;

		ret = _opp_set_voltage(dev, new_supply_vbb, 0, vbb_reg, "vbb");
		if (ret)
			goto restore_voltage;
	}

	/* Change frequency */
	dev_dbg(dev, "%s: switching OPP: %lu Hz --> %lu Hz\n",
		__func__, old_freq, freq);

	ret = clk_set_rate(clk, freq);
	if (ret) {
		dev_err(dev, "%s: failed to set clock rate: %d\n", __func__,
			ret);
		goto restore_voltage;
	}

	/* Scaling down? Scale voltage after frequency */
	if (freq < old_freq) {
		ret = _opp_set_voltage(dev, new_supply_vbb, 0, vbb_reg, "vbb");
		if (ret)
			goto restore_freq;

		ret = _opp_set_voltage(dev, new_supply_vdd, vdd_uv, vdd_reg,
				       "vdd");
		if (ret)
			goto restore_freq;
	}

	return 0;

restore_freq:
	ret = clk_set_rate(clk, old_freq);
	if (ret)
		dev_err(dev, "%s: failed to restore old-freq (%lu Hz)\n",
			__func__, old_freq);
restore_voltage:
	/* This shouldn't harm even if the voltages weren't updated earlier */
	if (old_supply_vdd->u_volt) {
		ret = _opp_set_voltage(dev, old_supply_vbb, 0, vbb_reg, "vbb");
		if (ret)
			return ret;

		ret = _opp_set_voltage(dev, old_supply_vdd, 0, vdd_reg,
				       "vdd");
		if (ret)
			return ret;
	}

	return ret;
}