[PATCH v2 2/4] clk: conf: Support assigned-clock-sscs

["Peng Fan (OSS)" <peng.fan@oss.nxp.com>]

From: Peng Fan <peng.fan@nxp.com>

Parse the Spread Spectrum Configuration(SSC) from device tree and configure
them before using the clock.

Each SSC is three u32 elements which means '<modfreq spreaddepth
modmethod>', so assigned-clock-sscs is an array of multiple three u32
elements.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
---
 drivers/clk/clk-conf.c | 70 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)

diff --git a/drivers/clk/clk-conf.c b/drivers/clk/clk-conf.c
index 303a0bb26e54a95655ce094a35b989c97ebc6fd8..9046a7710236839a30a72dc15c5fcebdbed5e9c6 100644
--- a/drivers/clk/clk-conf.c
+++ b/drivers/clk/clk-conf.c
@@ -155,6 +155,72 @@ static int __set_clk_rates(struct device_node *node, bool clk_supplier)
 	return 0;
 }
 
+static int __set_clk_spread_spectrum(struct device_node *node, bool clk_supplier)
+{
+	u32 *sscs __free(kfree) = NULL;
+	u32 elem_size = sizeof(u32) * 3;
+	struct of_phandle_args clkspec;
+	int rc, count, index;
+	struct clk *clk;
+
+	/* modfreq, spreadPercent, modmethod */
+	count = of_property_count_elems_of_size(node, "assigned-clock-sscs", elem_size);
+	if (count > 0) {
+		sscs = kcalloc(count, elem_size, GFP_KERNEL);
+		if (!sscs)
+			return -ENOMEM;
+		rc = of_property_read_u32_array(node,
+						"assigned-clock-sscs",
+						sscs, count * 3);
+	} else {
+		return 0;
+	}
+
+	if (rc)
+		return rc;
+
+	for (index = 0; index < count; index++) {
+		u32 modfreq = sscs[index * 3], spreaddepth = sscs[index * 3 + 1];
+		u32 method = sscs[index * 3 + 2];
+		struct clk_hw *hw;
+
+		if (modfreq || spreaddepth || method) {
+			rc = of_parse_phandle_with_args(node, "assigned-clocks",
+					"#clock-cells",	index, &clkspec);
+			if (rc < 0) {
+				/* skip empty (null) phandles */
+				if (rc == -ENOENT)
+					continue;
+				else
+					return rc;
+			}
+
+			if (clkspec.np == node && !clk_supplier) {
+				of_node_put(clkspec.np);
+				return 0;
+			}
+
+			clk = of_clk_get_from_provider(&clkspec);
+			of_node_put(clkspec.np);
+			if (IS_ERR(clk)) {
+				if (PTR_ERR(clk) != -EPROBE_DEFER)
+					pr_warn("clk: couldn't get clock %d for %pOF\n",
+						index, node);
+				return PTR_ERR(clk);
+			}
+
+			hw = __clk_get_hw(clk);
+			rc = clk_hw_set_spread_spectrum(hw, modfreq, spreaddepth, method, true);
+			if (rc < 0)
+				pr_err("clk: couldn't set %s clk spread spectrum %u %u %u: %d\n",
+				       __clk_get_name(clk), modfreq, spreaddepth, method, rc);
+			clk_put(clk);
+		}
+	}
+
+	return 0;
+}
+
 /**
  * of_clk_set_defaults() - parse and set assigned clocks configuration
  * @node: device node to apply clock settings for
@@ -174,6 +240,10 @@ int of_clk_set_defaults(struct device_node *node, bool clk_supplier)
 	if (!node)
 		return 0;
 
+	rc = __set_clk_spread_spectrum(node, clk_supplier);
+	if (rc < 0)
+		return rc;
+
 	rc = __set_clk_parents(node, clk_supplier);
 	if (rc < 0)
 		return rc;

-- 
2.37.1