clk: qcom: Add support for debugfs measure clock

Introduce the clk_debug_mux which would support clock
frequency measurement from debugfs.

Change-Id: I81c32a876b33f5a7773485a76897ff9cbed45a76
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

@@ -62,8 +62,8 @@ struct clk_core {
 	struct clk_core		*parent;
 	const char		**parent_names;
 	struct clk_core		**parents;
-	u8			num_parents;
-	u8			new_parent_index;
+	unsigned int		num_parents;
+	unsigned int		new_parent_index;
 	unsigned long		rate;
 	unsigned long		req_rate;
 	unsigned long		new_rate;
@@ -2563,7 +2563,7 @@ static int clk_core_set_parent_nolock(struct clk_core *core,
 	if (!core)
 		return 0;
 
-	if (core->parent == parent)
+	if (core->parent == parent && !(core->flags & CLK_IS_MEASURE))
 		return 0;
 
 	/* verify ops for for multi-parent clks */

drivers/clk/qcom/Makefile

@@ -12,7 +12,7 @@ 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-voter.o
-clk-qcom-y += clk-dummy.o
+clk-qcom-y += clk-dummy.o clk-debug.o
 clk-qcom-y += gdsc-regulator.o
 clk-qcom-$(CONFIG_QCOM_GDSC) += gdsc.o
 

drivers/clk/qcom/clk-branch.c

@@ -15,6 +15,7 @@
 
 #include "clk-branch.h"
 #include "clk-regmap.h"
+#include "clk-debug.h"
 
 static bool clk_branch_in_hwcg_mode(const struct clk_branch *br)
 {
@@ -363,6 +364,7 @@ const struct clk_ops clk_branch2_ops = {
 	.recalc_rate = clk_branch2_recalc_rate,
 	.set_flags = clk_branch_set_flags,
 	.list_registers = clk_branch2_list_registers,
+	.debug_init = clk_debug_measure_add,
 };
 EXPORT_SYMBOL_GPL(clk_branch2_ops);
 

drivers/clk/qcom/clk-debug.c

@@ -0,0 +1,290 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (c) 2016, The Linux Foundation. All rights reserved. */
+
+#include <linux/clk.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/platform_device.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/bitops.h>
+
+#include "clk-regmap.h"
+#include "clk-debug.h"
+
+static struct clk_hw *measure;
+
+static DEFINE_SPINLOCK(clk_reg_lock);
+static DEFINE_MUTEX(clk_debug_lock);
+
+#define TCXO_DIV_4_HZ		4800000
+#define SAMPLE_TICKS_1_MS	0x1000
+#define SAMPLE_TICKS_14_MS	0x10000
+
+#define XO_DIV4_CNT_DONE	BIT(25)
+#define CNT_EN			BIT(20)
+#define MEASURE_CNT		GENMASK(24, 0)
+
+/* Sample clock for 'ticks' reference clock ticks. */
+static u32 run_measurement(unsigned int 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 & XO_DIV4_CNT_DONE) != 0) {
+		cpu_relax();
+		regmap_read(regmap, status_reg, &regval);
+	}
+
+	/* Run measurement and wait for completion. */
+	regmap_write(regmap, ctl_reg, (CNT_EN|ticks));
+
+	regmap_read(regmap, status_reg, &regval);
+
+	while ((regval & XO_DIV4_CNT_DONE) == 0) {
+		cpu_relax();
+		regmap_read(regmap, status_reg, &regval);
+	}
+
+	/* Return measured ticks. */
+	regmap_read(regmap, status_reg, &regval);
+	regval &= MEASURE_CNT;
+
+	/* Stop the counters */
+	regmap_write(regmap, ctl_reg, ticks);
+
+	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, 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;
+
+	clk_prepare_enable(data->cxo);
+
+	spin_lock_irqsave(&clk_reg_lock, flags);
+
+	/* 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(SAMPLE_TICKS_1_MS, meas->regmap[GCC],
+				data->ctl_reg, data->status_reg);
+
+	/* Run a full measurement. (~14 ms) */
+	raw_count_full = run_measurement(SAMPLE_TICKS_14_MS, 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) * TCXO_DIV_4_HZ;
+		do_div(raw_count_full, ((SAMPLE_TICKS_14_MS * 10) + 35));
+		ret = (raw_count_full * multiplier);
+	}
+
+	spin_unlock_irqrestore(&clk_reg_lock, flags);
+
+	clk_disable_unprepare(data->cxo);
+
+	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);
+	struct clk_hw *hw_clk = clk_hw_get_parent(hw);
+
+	if (!hw_clk)
+		return 0;
+
+	for (i = 0; i < num_parents; i++) {
+		if (!strcmp(meas->parent[i].parents,
+					clk_hw_get_name(hw_clk))) {
+			pr_debug("%s: clock parent - %s, index %d\n", __func__,
+					meas->parent[i].parents, 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) {
+		/* Update the recursive debug mux */
+		regmap_read(meas->regmap[dbg_cc],
+				meas->parent[index].mux_offset, &regval);
+		regval &= ~(meas->parent[index].mux_sel_mask <<
+				meas->parent[index].mux_sel_shift);
+		regval |= (meas->parent[index].dbg_cc_mux_sel &
+				meas->parent[index].mux_sel_mask) <<
+				meas->parent[index].mux_sel_shift;
+		regmap_write(meas->regmap[dbg_cc],
+				meas->parent[index].mux_offset, regval);
+
+		regmap_read(meas->regmap[dbg_cc],
+				meas->parent[index].post_div_offset, &regval);
+		regval &= ~(meas->parent[index].post_div_mask <<
+				meas->parent[index].post_div_shift);
+		regval |= ((meas->parent[index].post_div_val - 1) &
+				meas->parent[index].post_div_mask) <<
+				meas->parent[index].post_div_shift;
+		regmap_write(meas->regmap[dbg_cc],
+				meas->parent[index].post_div_offset, regval);
+
+		/* Not all recursive muxes have a DEBUG clock. */
+		if (meas->parent[index].cbcr_offset != U32_MAX) {
+			regmap_read(meas->regmap[dbg_cc],
+				meas->parent[index].cbcr_offset, &regval);
+			regval |= BIT(0);
+			regmap_write(meas->regmap[dbg_cc],
+				meas->parent[index].cbcr_offset, regval);
+		}
+	}
+
+	/* Update the debug sel for GCC */
+	regmap_read(meas->regmap[GCC], meas->debug_offset, &regval);
+	regval &= ~(meas->src_sel_mask << meas->src_sel_shift);
+	regval |= (meas->parent[index].prim_mux_sel & meas->src_sel_mask) <<
+			meas->src_sel_shift;
+	regmap_write(meas->regmap[GCC], meas->debug_offset, regval);
+
+	/* Set the GCC mux's post divider bits */
+	regmap_read(meas->regmap[GCC], meas->post_div_offset, &regval);
+	regval &= ~(meas->post_div_mask << meas->post_div_shift);
+	regval |= ((meas->parent[index].prim_mux_div_val - 1) &
+			meas->post_div_mask) << meas->post_div_shift;
+	regmap_write(meas->regmap[GCC], meas->post_div_offset, regval);
+
+	/* Turn on the GCC_DEBUG_CBCR */
+	regmap_read(meas->regmap[GCC], meas->cbcr_offset, &regval);
+	regval |= BIT(0);
+	regmap_write(meas->regmap[GCC], meas->cbcr_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(clk_debug_mux_ops);
+
+static int clk_debug_measure_get(void *data, u64 *val)
+{
+	struct clk_hw *hw = data, *par;
+	struct clk_debug_mux *meas = to_clk_measure(measure);
+	int index;
+	int ret = 0;
+	unsigned long meas_rate, sw_rate;
+
+	mutex_lock(&clk_debug_lock);
+
+	ret = clk_set_parent(measure->clk, hw->clk);
+	if (!ret) {
+		par = measure;
+		index =  clk_debug_mux_get_parent(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);
+		if (meas->parent[index].dbg_cc != GCC)
+			*val *= meas->parent[index].post_div_val;
+		*val *= meas->parent[index].prim_mux_div_val;
+
+		/* Accommodate for any pre-set dividers */
+		if (meas->parent[index].misc_div_val)
+			*val *= meas->parent[index].misc_div_val;
+	}
+
+	meas_rate = clk_get_rate(hw->clk);
+	par = clk_hw_get_parent(measure);
+	if (!par)
+		return -EINVAL;
+
+	sw_rate = clk_get_rate(par->clk);
+	if (sw_rate && meas_rate >= (sw_rate * 2))
+		*val *= DIV_ROUND_CLOSEST(meas_rate, sw_rate);
+	mutex_unlock(&clk_debug_lock);
+
+	return ret;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(clk_measure_fops, clk_debug_measure_get,
+							NULL, "%lld\n");
+
+void clk_debug_measure_add(struct clk_hw *hw, struct dentry *dentry)
+{
+	int ret;
+
+	if (IS_ERR_OR_NULL(measure)) {
+		pr_err_once("Please check if `measure` clk is registered.\n");
+		return;
+	}
+
+	ret = clk_set_parent(measure->clk, hw->clk);
+	if (ret) {
+		pr_debug("Unable to set %s as %s's parent, ret=%d\n",
+			clk_hw_get_name(hw), clk_hw_get_name(measure), ret);
+		return;
+	}
+
+	debugfs_create_file("clk_measure", 0x444, dentry, hw,
+					&clk_measure_fops);
+}
+EXPORT_SYMBOL(clk_debug_measure_add);
+
+int clk_debug_measure_register(struct clk_hw *hw)
+{
+	if (IS_ERR_OR_NULL(measure)) {
+		if (hw->init->flags & CLK_IS_MEASURE) {
+			measure = hw;
+			return 0;
+		}
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(clk_debug_measure_register);
+

drivers/clk/qcom/clk-debug.h

@@ -0,0 +1,126 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2016, The Linux Foundation. All rights reserved. */
+
+#ifndef __QCOM_CLK_DEBUG_H__
+#define __QCOM_CLK_DEBUG_H__
+
+#include "../clk.h"
+
+/* 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.
+ * @cbcr_offset:	branch register to turn on debug mux.
+ */
+struct measure_clk_data {
+	struct clk *cxo;
+	u32 ctl_reg;
+	u32 status_reg;
+	u32 xo_div4_cbcr;
+};
+
+/**
+ * List of Debug clock controllers.
+ */
+enum debug_cc {
+	GCC,
+	CAM_CC,
+	DISP_CC,
+	NPU_CC,
+	GPU_CC,
+	VIDEO_CC,
+	CPU_CC,
+	MAX_NUM_CC,
+};
+
+/**
+ * struct clk_src - Structure of clock source for debug mux
+ *
+ * @parents:		clock name to be used as parent for debug mux.
+ * @prim_mux_sel:	debug mux index at global clock controller.
+ * @prim_mux_div_val:	PLL post-divider setting for the primary mux.
+ * @dbg_cc:		indicates the clock controller for recursive debug
+ *			clock controllers.
+ * @dbg_cc_mux_sel:	indicates the debug mux index at recursive debug mux.
+ * @mux_sel_mask:	indicates the mask for the mux selection.
+ * @mux_sel_shift:	indicates the shift required for mux selection.
+ * @post_div_mask:	indicates the post div mask to be used at recursive
+ *			debug mux.
+ * @post_div_shift:	indicates the shift required for post divider
+ *			configuration.
+ * @post_div_val:	indicates the post div value to be used at recursive
+ *			debug mux.
+ * @mux_offset:		the debug mux offset.
+ * @post_div_offset:	register with post-divider settings for the debug mux.
+ * @cbcr_offset:	branch register to turn on debug mux.
+ * @misc_div_val:	includes any pre-set dividers in the measurement logic.
+ */
+struct clk_src {
+	const char *parents;
+	int prim_mux_sel;
+	u32 prim_mux_div_val;
+	enum debug_cc dbg_cc;
+	int dbg_cc_mux_sel;
+	u32 mux_sel_mask;
+	u32 mux_sel_shift;
+	u32 post_div_mask;
+	u32 post_div_shift;
+	u32 post_div_val;
+	u32 mux_offset;
+	u32 post_div_offset;
+	u32 cbcr_offset;
+	u32 misc_div_val;
+};
+
+#define MUX_SRC_LIST(...) \
+	.parent = (struct clk_src[]){__VA_ARGS__}, \
+	.num_parents = ARRAY_SIZE(((struct clk_src[]){__VA_ARGS__}))
+
+/**
+ * struct clk_debug_mux - Structure of clock debug mux
+ *
+ * @parent:		structure of clk_src
+ * @num_parents:	number of parents
+ * @regmap:		regmaps of debug mux
+ * @priv:		private measure_clk_data to be used by debug mux
+ * @debug_offset:	debug mux offset.
+ * @post_div_offset:	register with post-divider settings for the debug mux.
+ * @cbcr_offset:	branch register to turn on debug mux.
+ * @src_sel_mask:	indicates the mask to be used for src selection in
+			primary mux.
+ * @src_sel_shift:	indicates the shift required for source selection in
+			primary mux.
+ * @post_div_mask:	indicates the post div mask to be used for the primary
+			mux.
+ * @post_div_shift:	indicates the shift required for post divider
+			selection in primary mux.
+ * @hw:			handle between common and hardware-specific interfaces.
+ */
+struct clk_debug_mux {
+	struct clk_src *parent;
+	int num_parents;
+	struct regmap **regmap;
+	void *priv;
+	u32 debug_offset;
+	u32 post_div_offset;
+	u32 cbcr_offset;
+	u32 src_sel_mask;
+	u32 src_sel_shift;
+	u32 post_div_mask;
+	u32 post_div_shift;
+	struct clk_hw hw;
+};
+
+#define to_clk_measure(_hw) container_of((_hw), struct clk_debug_mux, hw)
+
+extern const struct clk_ops clk_debug_mux_ops;
+
+int clk_debug_measure_register(struct clk_hw *hw);
+void clk_debug_measure_add(struct clk_hw *hw, struct dentry *dentry);
+
+#endif

drivers/clk/qcom/clk-dummy.c

@@ -9,6 +9,7 @@
 #include <linux/slab.h>
 
 #include "common.h"
+#include "clk-debug.h"
 
 #define to_clk_dummy(_hw)	container_of(_hw, struct clk_dummy, hw)
 
@@ -53,6 +54,7 @@ const struct clk_ops clk_dummy_ops = {
 	.round_rate = dummy_clk_round_rate,
 	.recalc_rate = dummy_clk_recalc_rate,
 	.set_flags = dummy_clk_set_flags,
+	.debug_init = clk_debug_measure_add,
 };
 EXPORT_SYMBOL(clk_dummy_ops);
 

include/linux/clk-provider.h

@@ -46,6 +46,7 @@
 					 * hand-off enable_count & prepare_count
 					 * to first consumer that enables clk
 					 */
+#define CLK_IS_MEASURE          BIT(14) /* measure clock */
 
 struct clk;
 struct clk_hw;
@@ -285,7 +286,7 @@ struct clk_init_data {
 	const char		*name;
 	const struct clk_ops	*ops;
 	const char		* const *parent_names;
-	u8			num_parents;
+	unsigned int		num_parents;
 	unsigned long		flags;
 	struct clk_vdd_class	*vdd_class;
 	unsigned long		*rate_max;