clk: qcom: Add support for debugfs measure clock

Introduce clk_debug_mux which would support clocks to be allowed to measure
clock frequency from debugfs.

Change-Id: I81c32a876b33f5a7773485a76897ff9cbed45a76
Signed-off-by: Taniya Das <tdas@codeaurora.org>

drivers/clk/clk.c

@@ -2801,6 +2801,8 @@ static int clk_debug_create_one(struct clk_core *core, struct dentry *pdentry)
 			goto err_out;
 	}
 
+	clk_debug_measure_add(core->hw, core->dentry);
+
 	ret = 0;
 	goto out;
 
@@ -2930,8 +2932,10 @@ static int __init clk_debug_init(void)
 		return -ENOMEM;
 
 	mutex_lock(&clk_debug_lock);
-	hlist_for_each_entry(core, &clk_debug_list, debug_node)
+	hlist_for_each_entry(core, &clk_debug_list, debug_node) {
+		clk_register_debug(core->hw, core->dentry);
 		clk_debug_create_one(core, rootdir);
+	}
 
 	inited = 1;
 	mutex_unlock(&clk_debug_lock);

drivers/clk/clk.h

@@ -23,6 +23,8 @@ void __clk_free_clk(struct clk *clk);
 
 /* Debugfs API to print the enabled clocks */
 void clock_debug_print_enabled(void);
+int clk_register_debug(struct clk_hw *hw, struct dentry *dentry);
+void clk_debug_measure_add(struct clk_hw *hw, struct dentry *dentry);
 
 #else
 /* All these casts to avoid ifdefs in clkdev... */

drivers/clk/qcom/common.c

@@ -11,6 +11,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/clk.h>
 #include <linux/export.h>
 #include <linux/module.h>
 #include <linux/regmap.h>
@@ -286,4 +287,221 @@ int qcom_cc_probe(struct platform_device *pdev, const struct qcom_cc_desc *desc)
 }
 EXPORT_SYMBOL_GPL(qcom_cc_probe);
 
+/* Debugfs Support */
+static struct clk_hw *measure;
+
+DEFINE_SPINLOCK(clk_reg_lock);
+
+/* Sample clock for 'ticks' reference clock ticks. */
+static u32 run_measurement(unsigned ticks, struct regmap *regmap,
+		u32 ctl_reg, u32 status_reg)
+{
+	u32 regval;
+
+	/* Stop counters and set the XO4 counter start value. */
+	regmap_write(regmap, ctl_reg, ticks);
+
+	regmap_read(regmap, status_reg, &regval);
+
+	/* Wait for timer to become ready. */
+	while ((regval & BIT(25)) != 0) {
+		cpu_relax();
+		regmap_read(regmap, status_reg, &regval);
+	}
+
+	/* Run measurement and wait for completion. */
+	regmap_write(regmap, ctl_reg, (BIT(20)|ticks));
+	regmap_read(regmap, ctl_reg, &regval);
+
+	regmap_read(regmap, status_reg, &regval);
+
+	while ((regval & BIT(25)) == 0) {
+		cpu_relax();
+		regmap_read(regmap, status_reg, &regval);
+	}
+
+	/* Return measured ticks. */
+	regmap_read(regmap, status_reg, &regval);
+	regval &= BM(24, 0);
+
+	return regval;
+}
+
+/*
+ * Perform a hardware rate measurement for a given clock.
+ * FOR DEBUG USE ONLY: Measurements take ~15 ms!
+ */
+static unsigned long clk_debug_mux_measure_rate(struct clk_hw *hw)
+{
+	unsigned long flags, ret = 0;
+	u32 gcc_xo4_reg, sample_ticks = 0x10000, multiplier = 1;
+	u64 raw_count_short, raw_count_full;
+	struct clk_debug_mux *meas = to_clk_measure(hw);
+	struct measure_clk_data *data = meas->priv;
+
+	spin_lock_irqsave(&clk_reg_lock, flags);
+
+	clk_prepare_enable(data->cxo);
+
+	/* Enable CXO/4 and RINGOSC branch. */
+	regmap_read(meas->regmap[GCC], data->xo_div4_cbcr, &gcc_xo4_reg);
+	gcc_xo4_reg |= BIT(0);
+	regmap_write(meas->regmap[GCC], data->xo_div4_cbcr, gcc_xo4_reg);
+
+	/*
+	 * The ring oscillator counter will not reset if the measured clock
+	 * is not running.  To detect this, run a short measurement before
+	 * the full measurement.  If the raw results of the two are the same
+	 * then the clock must be off.
+	 */
+
+	/* Run a short measurement. (~1 ms) */
+	raw_count_short = run_measurement(0x1000, meas->regmap[GCC],
+					data->ctl_reg, data->status_reg);
+
+	/* Run a full measurement. (~14 ms) */
+	raw_count_full = run_measurement(sample_ticks, meas->regmap[GCC],
+					data->ctl_reg, data->status_reg);
+
+	gcc_xo4_reg &= ~BIT(0);
+	regmap_write(meas->regmap[GCC], data->xo_div4_cbcr, gcc_xo4_reg);
+
+	/* Return 0 if the clock is off. */
+	if (raw_count_full == raw_count_short)
+		ret = 0;
+	else {
+		/* Compute rate in Hz. */
+		raw_count_full = ((raw_count_full * 10) + 15) * 4800000;
+		do_div(raw_count_full, ((sample_ticks * 10) + 35));
+		ret = (raw_count_full * multiplier);
+	}
+
+	clk_disable_unprepare(data->cxo);
+
+	spin_unlock_irqrestore(&clk_reg_lock, flags);
+
+	return ret;
+}
+
+static u8 clk_debug_mux_get_parent(struct clk_hw *hw)
+{
+	struct clk_debug_mux *meas = to_clk_measure(hw);
+	int i, num_parents = clk_hw_get_num_parents(hw);
+
+	for (i = 0; i < num_parents; i++) {
+		if (!strcmp(meas->parent[i].parents,
+					hw->init->parent_names[i])) {
+			pr_debug("%s :Clock name %s index %d\n", __func__,
+					hw->init->name, i);
+			return i;
+		}
+	}
+
+	return 0;
+}
+
+static int clk_debug_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_debug_mux *meas = to_clk_measure(hw);
+	u32 regval = 0;
+	int dbg_cc = 0;
+
+	dbg_cc = meas->parent[index].dbg_cc;
+
+	if (dbg_cc != GCC) {
+		regmap_read(meas->regmap[dbg_cc], 0x0, &regval);
+
+		if (meas->parent[index].mask)
+			regval &= ~meas->parent[index].mask <<
+					meas->parent[index].shift;
+		else
+			regval &= ~meas->mask;
+
+		regval |= (meas->parent[index].next_sel & meas->mask);
+
+		if (meas->parent[index].en_mask == 0xFF)
+			/* Skip en_mask */
+			regval = regval;
+		else if (meas->parent[index].en_mask)
+			regval |= meas->parent[index].en_mask;
+		else
+			regval |= meas->en_mask;
+
+		regmap_write(meas->regmap[dbg_cc], 0x0, regval);
+	}
+
+       /* update the debug sel for GCC */
+	regmap_read(meas->regmap[GCC], meas->debug_offset, &regval);
+
+	/* clear post divider bits */
+	regval &= ~BM(15, 12);
+	regval &= ~meas->mask;
+	regval |= (meas->parent[index].sel & meas->mask);
+	regval |= meas->en_mask;
+
+	regmap_write(meas->regmap[GCC], meas->debug_offset, regval);
+
+	return 0;
+}
+
+const struct clk_ops clk_debug_mux_ops = {
+	.get_parent = clk_debug_mux_get_parent,
+	.set_parent = clk_debug_mux_set_parent,
+};
+EXPORT_SYMBOL_GPL(clk_debug_mux_ops);
+
+static int clk_debug_measure_get(void *data, u64 *val)
+{
+	struct clk_hw *hw = data, *par;
+	int ret = 0;
+	unsigned long meas_rate, sw_rate;
+
+	ret = clk_set_parent(measure->clk, hw->clk);
+	if (!ret) {
+		par = measure;
+		while (par && par != hw) {
+			if (par->init->ops->enable)
+				par->init->ops->enable(par);
+			par = clk_hw_get_parent(par);
+		}
+		*val = clk_debug_mux_measure_rate(measure);
+	}
+
+	meas_rate = clk_get_rate(hw->clk);
+	sw_rate = clk_get_rate(clk_hw_get_parent(measure)->clk);
+	if (sw_rate && meas_rate >= (sw_rate * 2))
+		*val *= DIV_ROUND_CLOSEST(meas_rate, sw_rate);
+
+	return ret;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(clk_measure_fops, clk_debug_measure_get,
+							NULL, "%lld\n");
+
+void clk_debug_measure_add(struct clk_hw *hw, struct dentry *dentry)
+{
+	if (IS_ERR_OR_NULL(measure))
+		return;
+
+	if (clk_set_parent(measure->clk, hw->clk))
+		return;
+
+	debugfs_create_file("measure", S_IRUGO, dentry, hw,
+					&clk_measure_fops);
+}
+EXPORT_SYMBOL_GPL(clk_debug_measure_add);
+
+int clk_register_debug(struct clk_hw *hw, struct dentry *dentry)
+{
+	if (IS_ERR_OR_NULL(measure)) {
+		if (hw->init->flags & CLK_IS_MEASURE)
+			measure = hw;
+		if (!IS_ERR_OR_NULL(measure))
+			clk_debug_measure_add(hw, dentry);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(clk_register_debug);
+
 MODULE_LICENSE("GPL v2");

drivers/clk/qcom/common.h

@@ -51,4 +51,95 @@ extern int qcom_cc_really_probe(struct platform_device *pdev,
 extern int qcom_cc_probe(struct platform_device *pdev,
 			 const struct qcom_cc_desc *desc);
 extern struct clk_ops clk_dummy_ops;
+
+/* Debugfs Measure Clocks */
+
+/**
+ * struct measure_clk_data - Structure of clk measure
+ *
+ * @cxo:		XO clock.
+ * @xo_div4_cbcr:	offset of debug XO/4 div register.
+ * @ctl_reg:		offset of debug control register.
+ * @status_reg:		offset of debug status register.
+ *
+ */
+struct measure_clk_data {
+	struct clk *cxo;
+	u32 xo_div4_cbcr;
+	u32 ctl_reg;
+	u32 status_reg;
+};
+
+/**
+ * List of Debug clock controllers.
+ */
+enum debug_cc {
+	GCC,
+	MMCC,
+	GPU,
+	CPU,
+};
+
+/**
+ * struct clk_src - Struture of clock source for debug mux
+ *
+ * @parents:	clock name to be used as parent for debug mux.
+ * @sel:	debug mux index at global clock controller.
+ * @dbg_cc:     indicates the clock controller for recursive debug clock
+ *		controllers.
+ * @next_sel:	indicates the debug mux index at recursive debug mux.
+ * @mask:	indicates the mask required at recursive debug mux.
+ * @shift:	indicates the shift required at recursive debug mux.
+ * @en_mask:	indicates the enable bit mask at recursive debug mux.
+ *		Incase the recursive debug mux does not have a enable bit,
+ *		0xFF should be used to indicate the same, otherwise global
+ *		enable bit would be used.
+ */
+struct clk_src {
+	const char  *parents;
+	int sel;
+	enum debug_cc dbg_cc;
+	int next_sel;
+	u32 mask;
+	u32 shift;
+	u32 en_mask;
+};
+
+#define MUX_SRC_LIST(...) \
+	.parent = (struct clk_src[]){__VA_ARGS__}, \
+	.num_parents = ARRAY_SIZE(((struct clk_src[]){__VA_ARGS__}))
+
+/**
+ * struct clk_debug_mux - Struture of clock debug mux
+ *
+ * @parent:		structure of clk_src
+ * @num_parents:	number of parents
+ * @regmap:		regmaps of debug mux
+ * @num_parent_regmap:	number of regmap of debug mux
+ * @priv:		private measure_clk_data to be used by debug mux
+ * @en_mask:		indicates the enable bit mask at global clock
+ *			controller debug mux.
+ * @mask:		indicates the mask to be used at global clock
+ *			controller debug mux.
+ * @debug_offset:	Start of debug mux offset.
+ * @hw:			handle between common and hardware-specific interfaces.
+ */
+struct clk_debug_mux {
+	struct clk_src *parent;
+	int num_parents;
+	struct regmap **regmap;
+	int num_parent_regmap;
+	void *priv;
+	u32 en_mask;
+	u32 mask;
+	u32 debug_offset;
+	struct clk_hw hw;
+};
+
+#define BM(msb, lsb) (((((uint32_t)-1) << (31-msb)) >> (31-msb+lsb)) << lsb)
+
+#define to_clk_measure(_hw) container_of((_hw), struct clk_debug_mux, hw)
+
+extern const struct clk_ops clk_debug_mux_ops;
+
 #endif

include/linux/clk-provider.h

@@ -33,6 +33,7 @@
 #define CLK_GET_ACCURACY_NOCACHE BIT(8) /* do not use the cached clk accuracy */
 #define CLK_RECALC_NEW_RATES	BIT(9) /* recalc rates after notifications */
 #define CLK_IS_CRITICAL		BIT(11) /* do not gate, ever */
+#define CLK_IS_MEASURE          BIT(14) /* measure clock */
 
 struct clk;
 struct clk_hw;