clk: qcom: clk-voter: Add support for voter clocks

Voter clocks nodes would require aggregation of all child node rates.
Certain clocks that are not rate-settable can still take
advantage of voter clock functionality.

Change-Id: Ibab7a5aa6aa89236974fcd0d65ffe0bd1a7acb12
Signed-off-by: Taniya Das <tdas@codeaurora.org>
Signed-off-by: Deepak Katragadda <dkatraga@codeaurora.org>
Signed-off-by: David Dai <daidavid1@codeaurora.org>

drivers/clk/clk.c

@@ -521,6 +521,26 @@ void clk_hw_set_rate_range(struct clk_hw *hw, unsigned long min_rate,
 }
 EXPORT_SYMBOL_GPL(clk_hw_set_rate_range);
 
+/*
+ * Aggregate the rate of all the enabled child nodes and exclude that
+ * of the child node for which this request was made.
+ */
+unsigned long clk_aggregate_rate(struct clk_hw *hw,
+					const struct clk_core *parent)
+{
+	struct clk_core *child;
+	unsigned long aggre_rate = 0;
+
+	hlist_for_each_entry(child, &parent->children, child_node) {
+		if (child->enable_count &&
+				strcmp(child->name, hw->init->name))
+			aggre_rate = max(child->rate, aggre_rate);
+	}
+
+	return aggre_rate;
+}
+EXPORT_SYMBOL_GPL(clk_aggregate_rate);
+
 /*
  * Helper for finding best parent to provide a given frequency. This can be used
  * directly as a determine_rate callback (e.g. for a mux), or from a more

drivers/clk/qcom/Makefile

@@ -11,7 +11,7 @@ clk-qcom-y += clk-branch.o
 clk-qcom-y += clk-regmap-divider.o
 clk-qcom-y += clk-regmap-mux.o
 clk-qcom-y += clk-regmap-mux-div.o
-clk-qcom-y += reset.o
+clk-qcom-y += reset.o clk-voter.o
 clk-qcom-y += clk-dummy.o
 clk-qcom-y += gdsc-regulator.o
 clk-qcom-$(CONFIG_QCOM_GDSC) += gdsc.o

drivers/clk/qcom/clk-voter.c

@@ -0,0 +1,136 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2017, The Linux Foundation. All rights reserved.
+ */
+
+#include <linux/clk.h>
+
+#include "clk-voter.h"
+
+static int voter_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	int ret = 0;
+	struct clk_voter *v = to_clk_voter(hw);
+	unsigned long cur_rate, new_rate, other_rate = 0;
+
+	if (v->is_branch)
+		return ret;
+
+	if (v->enabled) {
+		struct clk_hw *parent = clk_hw_get_parent(hw);
+
+		if (!parent)
+			return -EINVAL;
+
+		/*
+		 * Get the aggregate rate without this clock's vote and update
+		 * if the new rate is different than the current rate.
+		 */
+		other_rate = clk_aggregate_rate(hw, parent->core);
+
+		cur_rate = max(other_rate, clk_get_rate(hw->clk));
+		new_rate = max(other_rate, rate);
+
+		if (new_rate != cur_rate) {
+			ret = clk_set_rate(parent->clk, new_rate);
+			if (ret)
+				return ret;
+		}
+	}
+	v->rate =  rate;
+
+	return ret;
+}
+
+static int voter_clk_prepare(struct clk_hw *hw)
+{
+	int ret = 0;
+	unsigned long cur_rate;
+	struct clk_hw *parent;
+	struct clk_voter *v = to_clk_voter(hw);
+
+	parent = clk_hw_get_parent(hw);
+	if (!parent)
+		return -EINVAL;
+
+	if (v->is_branch) {
+		v->enabled = true;
+		return ret;
+	}
+
+	/*
+	 * Increase the rate if this clock is voting for a higher rate
+	 * than the current rate.
+	 */
+	cur_rate = clk_aggregate_rate(hw, parent->core);
+
+	if (v->rate > cur_rate) {
+		ret = clk_set_rate(parent->clk, v->rate);
+		if (ret)
+			return ret;
+	}
+	v->enabled = true;
+
+	return ret;
+}
+
+static void voter_clk_unprepare(struct clk_hw *hw)
+{
+	unsigned long cur_rate, new_rate;
+	struct clk_hw *parent;
+	struct clk_voter *v = to_clk_voter(hw);
+
+
+	parent = clk_hw_get_parent(hw);
+	if (!parent)
+		return;
+	/*
+	 * Decrease the rate if this clock was the only one voting for
+	 * the highest rate.
+	 */
+	v->enabled = false;
+	if (v->is_branch)
+		return;
+
+	new_rate = clk_aggregate_rate(hw, parent->core);
+	cur_rate = max(new_rate, v->rate);
+
+	if (new_rate < cur_rate)
+		clk_set_rate(parent->clk, new_rate);
+}
+
+static int voter_clk_is_enabled(struct clk_hw *hw)
+{
+	struct clk_voter *v = to_clk_voter(hw);
+
+	return v->enabled;
+}
+
+static long voter_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
+
+	if (!parent_hw)
+		return -EINVAL;
+
+	return clk_hw_round_rate(parent_hw, rate);
+}
+
+static unsigned long voter_clk_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_voter *v = to_clk_voter(hw);
+
+	return v->rate;
+}
+
+const struct clk_ops clk_ops_voter = {
+	.prepare = voter_clk_prepare,
+	.unprepare = voter_clk_unprepare,
+	.set_rate = voter_clk_set_rate,
+	.is_enabled = voter_clk_is_enabled,
+	.round_rate = voter_clk_round_rate,
+	.recalc_rate = voter_clk_recalc_rate,
+};

drivers/clk/qcom/clk-voter.h

@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2017, The Linux Foundation. All rights reserved.
+ */
+
+#ifndef __QCOM_CLK_VOTER_H__
+#define __QCOM_CLK_VOTER_H__
+
+#include <linux/clk-provider.h>
+#include <linux/platform_device.h>
+
+struct clk_voter {
+	int is_branch;
+	bool enabled;
+	struct clk_hw hw;
+	unsigned long rate;
+};
+
+extern const struct clk_ops clk_ops_voter;
+
+#define to_clk_voter(_hw) container_of(_hw, struct clk_voter, hw)
+
+#define __DEFINE_CLK_VOTER(clk_name, _parent_name, _default_rate, _is_branch) \
+	struct clk_voter clk_name = {					 \
+		.is_branch = (_is_branch),				  \
+		.rate = _default_rate,					   \
+		.hw.init = &(struct clk_init_data){			   \
+			.ops = &clk_ops_voter,				   \
+			.name = #clk_name,				   \
+			.parent_names = (const char *[]){ #_parent_name }, \
+			.num_parents = 1,				   \
+		},							   \
+	}
+
+#define DEFINE_CLK_VOTER(clk_name, _parent_name, _default_rate) \
+	 __DEFINE_CLK_VOTER(clk_name, _parent_name, _default_rate, 0)
+
+#define DEFINE_CLK_BRANCH_VOTER(clk_name, _parent_name) \
+	 __DEFINE_CLK_VOTER(clk_name, _parent_name, 1000, 1)
+
+#endif

include/linux/clk-provider.h

@@ -802,6 +802,9 @@ void clk_hw_reparent(struct clk_hw *hw, struct clk_hw *new_parent);
 void clk_hw_set_rate_range(struct clk_hw *hw, unsigned long min_rate,
 			   unsigned long max_rate);
 
+unsigned long clk_aggregate_rate(struct clk_hw *hw,
+					const struct clk_core *parent);
+
 static inline void __clk_hw_set_clk(struct clk_hw *dst, struct clk_hw *src)
 {
 	dst->clk = src->clk;