static int __tpiu_enable_to_sdc(struct tpiu_drvdata *drvdata)
{
	int ret;

	if (!drvdata->reg || !drvdata->reg_io)
		return -EINVAL;

	ret = tpiu_reg_set_optimum_mode(drvdata->reg, drvdata->reg_hpm);
	if (ret < 0)
		return ret;
	ret = tpiu_reg_set_voltage(drvdata->reg, drvdata->reg_low,
				   drvdata->reg_high);
	if (ret)
		goto err0;
	ret = regulator_enable(drvdata->reg);
	if (ret)
		goto err1;
	ret = tpiu_reg_set_optimum_mode(drvdata->reg_io, drvdata->reg_hpm_io);
	if (ret < 0)
		goto err2;
	ret = tpiu_reg_set_voltage(drvdata->reg_io, drvdata->reg_low_io,
				   drvdata->reg_high_io);
	if (ret)
		goto err3;
	ret = regulator_enable(drvdata->reg_io);
	if (ret)
		goto err4;

	ret = clk_set_rate(drvdata->clk, CORESIGHT_CLK_RATE_FIXED);
	if (ret)
		goto err5;

	msm_tlmm_misc_reg_write(TLMM_SDC2_HDRV_PULL_CTL, 0x16D);
	msm_tlmm_misc_reg_write(TLMM_ETM_MODE_REG, 1);

	TPIU_UNLOCK(drvdata);

	tpiu_writel(drvdata, 0x8, TPIU_CURR_PORTSZ);
	tpiu_writel(drvdata, 0x103, TPIU_FFCR);

	TPIU_LOCK(drvdata);

	return 0;
err5:
	regulator_disable(drvdata->reg_io);
err4:
	tpiu_reg_set_voltage(drvdata->reg_io, 0, drvdata->reg_high_io);
err3:
	tpiu_reg_set_optimum_mode(drvdata->reg_io, 0);
err2:
	regulator_disable(drvdata->reg);
err1:
	tpiu_reg_set_voltage(drvdata->reg, 0, drvdata->reg_high);
err0:
	tpiu_reg_set_optimum_mode(drvdata->reg, 0);
	return ret;
}
static void __tpiu_disable_to_sdc(struct tpiu_drvdata *drvdata)
{
	msm_tlmm_misc_reg_write(TLMM_ETM_MODE_REG, 0);

	clk_set_rate(drvdata->clk, CORESIGHT_CLK_RATE_TRACE);

	regulator_disable(drvdata->reg);
	tpiu_reg_set_voltage(drvdata->reg, 0, drvdata->reg_high);
	tpiu_reg_set_optimum_mode(drvdata->reg, 0);

	regulator_disable(drvdata->reg_io);
	tpiu_reg_set_voltage(drvdata->reg_io, 0, drvdata->reg_high_io);
	tpiu_reg_set_optimum_mode(drvdata->reg_io, 0);
}
static int __tpiu_enable_to_sdc(struct tpiu_drvdata *drvdata)
{
	int ret;

	if (!drvdata->reg || !drvdata->reg_io)
		return -EINVAL;

	ret = tpiu_reg_set_optimum_mode(drvdata->reg, drvdata->reg_hpm);
	if (ret < 0)
		return ret;
	ret = tpiu_reg_set_voltage(drvdata->reg, drvdata->reg_low,
				   drvdata->reg_high);
	if (ret)
		goto err0;
	ret = regulator_enable(drvdata->reg);
	if (ret)
		goto err1;
	ret = tpiu_reg_set_optimum_mode(drvdata->reg_io, drvdata->reg_hpm_io);
	if (ret < 0)
		goto err2;
	ret = tpiu_reg_set_voltage(drvdata->reg_io, drvdata->reg_low_io,
				   drvdata->reg_high_io);
	if (ret)
		goto err3;
	ret = regulator_enable(drvdata->reg_io);
	if (ret)
		goto err4;

	ret = clk_set_rate(drvdata->clk, CORESIGHT_CLK_RATE_FIXED);
	if (ret)
		goto err5;

	return 0;
err5:
	regulator_disable(drvdata->reg_io);
err4:
	tpiu_reg_set_voltage(drvdata->reg_io, 0, drvdata->reg_high_io);
err3:
	tpiu_reg_set_optimum_mode(drvdata->reg_io, 0);
err2:
	regulator_disable(drvdata->reg);
err1:
	tpiu_reg_set_voltage(drvdata->reg, 0, drvdata->reg_high);
err0:
	tpiu_reg_set_optimum_mode(drvdata->reg, 0);
	return ret;
}