diff --git a/drivers/clk/qcom/clk-branch.c b/drivers/clk/qcom/clk-branch.c
index cffaf46d732f..096e16db02fe 100644
--- a/drivers/clk/qcom/clk-branch.c
+++ b/drivers/clk/qcom/clk-branch.c
@@ -248,6 +248,73 @@ const struct clk_ops clk_branch2_ops = {
 };
 EXPORT_SYMBOL_GPL(clk_branch2_ops);
 
+static int clk_branch2_hw_ctl_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	/*
+	 * Make sure the branch clock has CLK_SET_RATE_PARENT flag,
+	 * and the RCG has FORCE_ENABLE_RCGR flag set.
+	 */
+	if (!(hw->init->flags & CLK_SET_RATE_PARENT)) {
+		pr_err("set rate would not get propagated to parent\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static unsigned long clk_branch2_hw_ctl_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	return parent_rate;
+}
+
+static int clk_branch2_hw_ctl_determine_rate(struct clk_hw *hw,
+		struct clk_rate_request *req)
+{
+	struct clk_hw *clkp;
+
+	clkp = __clk_get_hw(clk_get_parent(hw->clk));
+
+	req->best_parent_hw = clkp;
+	req->best_parent_rate = clk_round_rate(clkp->clk, req->rate);
+
+	return 0;
+}
+
+static int clk_branch2_hw_ctl_enable(struct clk_hw *hw)
+{
+	struct clk_hw *parent = __clk_get_hw(clk_get_parent(hw->clk));
+
+	/* The parent branch clock should have been prepared prior to this. */
+	if (!parent || (parent && !clk_hw_is_prepared(parent)))
+		return -EINVAL;
+
+	return clk_enable_regmap(hw);
+}
+
+static void clk_branch2_hw_ctl_disable(struct clk_hw *hw)
+{
+	struct clk_hw *parent = __clk_get_hw(clk_get_parent(hw->clk));
+
+	if (!parent)
+		return;
+
+	clk_disable_regmap(hw);
+}
+
+const struct clk_ops clk_branch2_hw_ctl_ops = {
+	.enable = clk_branch2_hw_ctl_enable,
+	.disable = clk_branch2_hw_ctl_disable,
+	.is_enabled = clk_is_enabled_regmap,
+	.set_rate = clk_branch2_hw_ctl_set_rate,
+	.recalc_rate = clk_branch2_hw_ctl_recalc_rate,
+	.determine_rate = clk_branch2_hw_ctl_determine_rate,
+	.set_flags = clk_branch_set_flags,
+	.list_registers = clk_branch2_list_registers,
+};
+EXPORT_SYMBOL_GPL(clk_branch2_hw_ctl_ops);
+
 static int clk_gate_toggle(struct clk_hw *hw, bool en)
 {
 	struct clk_gate2 *gt = to_clk_gate2(hw);
diff --git a/drivers/clk/qcom/clk-branch.h b/drivers/clk/qcom/clk-branch.h
index 8a934cf8bed1..b67ac1dfbbf9 100644
--- a/drivers/clk/qcom/clk-branch.h
+++ b/drivers/clk/qcom/clk-branch.h
@@ -62,6 +62,7 @@ extern const struct clk_ops clk_branch_ops;
 extern const struct clk_ops clk_branch2_ops;
 extern const struct clk_ops clk_gate2_ops;
 extern const struct clk_ops clk_branch_simple_ops;
+extern const struct clk_ops clk_branch2_hw_ctl_ops;
 
 #define to_clk_branch(_hw) \
 	container_of(to_clk_regmap(_hw), struct clk_branch, clkr)
diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index 020bd351bbd8..accdac9fb964 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -171,10 +171,12 @@ struct clk_rcg2 {
 	const struct parent_map	*parent_map;
 	const struct freq_tbl	*freq_tbl;
 	unsigned long		current_freq;
+	u32		new_index;
+	u32		curr_index;
 	struct clk_regmap	clkr;
 
-#define FORCE_ENABLE_RCGR	BIT(0)
 	u8 flags;
+#define FORCE_ENABLE_RCGR	BIT(0)
 };
 
 #define to_clk_rcg2(_hw) container_of(to_clk_regmap(_hw), struct clk_rcg2, clkr)
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index 4104a238c088..653722f9c4b0 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -89,30 +89,6 @@ static int clk_rcg_set_force_enable(struct clk_hw *hw)
 	return ret;
 }
 
-static int clk_rcg2_enable(struct clk_hw *hw)
-{
-	int ret = 0;
-	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
-
-	if (rcg->flags & FORCE_ENABLE_RCGR)
-		ret = clk_rcg_set_force_enable(hw);
-
-	return ret;
-}
-
-static void clk_rcg2_disable(struct clk_hw *hw)
-{
-	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
-
-	if (rcg->flags & FORCE_ENABLE_RCGR) {
-		/* force disable RCG - clear CMD_ROOT_EN bit */
-		regmap_update_bits(rcg->clkr.regmap,
-			rcg->cmd_rcgr + CMD_REG, CMD_ROOT_EN, 0);
-		/* Add a delay to disable the RCG */
-		udelay(100);
-	}
-}
-
 static u8 clk_rcg2_get_parent(struct clk_hw *hw)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
@@ -381,16 +357,178 @@ static long clk_rcg2_list_rate(struct clk_hw *hw, unsigned n,
 	return (rcg->freq_tbl + n)->freq;
 }
 
+static int prepare_enable_rcg_srcs(struct clk_hw *hw, struct clk *curr,
+					struct clk *new)
+{
+	int rc = 0;
+
+	rc = clk_prepare(curr);
+	if (rc)
+		return rc;
+
+	if (clk_hw_is_prepared(hw)) {
+		rc = clk_prepare(new);
+		if (rc)
+			goto err_new_src_prepare;
+	}
+
+	rc = clk_prepare(new);
+	if (rc)
+		goto err_new_src_prepare2;
+
+	rc = clk_enable(curr);
+	if (rc)
+		goto err_curr_src_enable;
+
+	if (__clk_get_enable_count(hw->clk)) {
+		rc = clk_enable(new);
+		if (rc)
+			goto err_new_src_enable;
+	}
+
+	rc = clk_enable(new);
+	if (rc)
+		goto err_new_src_enable2;
+
+	return rc;
+
+err_new_src_enable2:
+	if (__clk_get_enable_count(hw->clk))
+		clk_disable(new);
+err_new_src_enable:
+	clk_disable(curr);
+err_curr_src_enable:
+	clk_unprepare(new);
+err_new_src_prepare2:
+	if (clk_hw_is_prepared(hw))
+		clk_unprepare(new);
+err_new_src_prepare:
+	clk_unprepare(curr);
+
+	return rc;
+}
+
+static void disable_unprepare_rcg_srcs(struct clk_hw *hw, struct clk *curr,
+					struct clk *new)
+{
+	clk_disable(new);
+
+	clk_disable(curr);
+
+	if (__clk_get_enable_count(hw->clk))
+		clk_disable(new);
+
+	clk_unprepare(new);
+	clk_unprepare(curr);
+
+	if (clk_hw_is_prepared(hw))
+		clk_unprepare(new);
+}
+
+static struct freq_tbl cxo_f = {
+	.freq = 19200000,
+	.src = 0,
+	.pre_div = 1,
+	.m = 0,
+	.n = 0,
+};
+
+static int clk_enable_disable_prepare_unprepare(struct clk_hw *hw, int cindex,
+			int nindex, bool enable)
+{
+	struct clk_hw *new_p, *curr_p;
+
+	curr_p = clk_hw_get_parent_by_index(hw, cindex);
+	new_p = clk_hw_get_parent_by_index(hw, nindex);
+
+	if (enable)
+		return prepare_enable_rcg_srcs(hw, curr_p->clk, new_p->clk);
+
+	disable_unprepare_rcg_srcs(hw, curr_p->clk, new_p->clk);
+	return 0;
+}
+
+static int clk_rcg2_enable(struct clk_hw *hw)
+{
+	int ret = 0;
+	const struct freq_tbl *f;
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+
+	if (rcg->flags & FORCE_ENABLE_RCGR) {
+		if (!rcg->current_freq)
+			rcg->current_freq = cxo_f.freq;
+
+		if (rcg->current_freq == cxo_f.freq)
+			rcg->curr_index = 0;
+		else {
+			f = qcom_find_freq(rcg->freq_tbl, rcg->current_freq);
+			rcg->curr_index = qcom_find_src_index(hw,
+						rcg->parent_map, f->src);
+		}
+
+		ret = clk_enable_disable_prepare_unprepare(hw, rcg->curr_index,
+					rcg->new_index, true);
+		if (ret) {
+			pr_err("Failed to prepare_enable new and current sources\n");
+			return ret;
+		}
+
+		clk_rcg_set_force_enable(hw);
+
+		clk_enable_disable_prepare_unprepare(hw, rcg->curr_index,
+					rcg->new_index, false);
+	}
+
+	return ret;
+}
+
+static void clk_rcg2_disable(struct clk_hw *hw)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+
+	if (rcg->flags & FORCE_ENABLE_RCGR) {
+		/* force disable RCG - clear CMD_ROOT_EN bit */
+		regmap_update_bits(rcg->clkr.regmap,
+			rcg->cmd_rcgr + CMD_REG, CMD_ROOT_EN, 0);
+		/* Add a delay to disable the RCG */
+		udelay(100);
+	}
+}
+
+
 static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f;
+	int ret = 0;
+
+	/* Current frequency */
+	if (rcg->flags & FORCE_ENABLE_RCGR)
+		rcg->current_freq = clk_get_rate(hw->clk);
 
 	f = qcom_find_freq(rcg->freq_tbl, rate);
 	if (!f)
 		return -EINVAL;
 
-	return clk_rcg2_configure(rcg, f);
+	/* New parent index */
+	if (rcg->flags & FORCE_ENABLE_RCGR) {
+		rcg->new_index = qcom_find_src_index(hw,
+					rcg->parent_map, f->src);
+		ret = clk_rcg2_enable(hw);
+		if (ret) {
+			pr_err("Failed to enable rcg\n");
+			return ret;
+		}
+	}
+
+	ret = clk_rcg2_configure(rcg, f);
+	if (ret)
+		return ret;
+
+	if (rcg->flags & FORCE_ENABLE_RCGR)
+		clk_rcg2_disable(hw);
+
+	return ret;
 }
 
 static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,