Subject: [PATCH] cpts refclk sel

Signed-off-by: Grygorii Strashko <grygorii.strashko@ti.com>
---
 arch/arm/boot/dts/keystone-k2e-netcp.dtsi | 10 +++++-
 drivers/net/ethernet/ti/cpts.c            | 52 ++++++++++++++++++++++++++++++-
 2 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/arch/arm/boot/dts/keystone-k2e-netcp.dtsi b/arch/arm/boot/dts/keystone-k2e-netcp.dtsi
index 919e655..b27aa22 100644
--- a/arch/arm/boot/dts/keystone-k2e-netcp.dtsi
+++ b/arch/arm/boot/dts/keystone-k2e-netcp.dtsi

@@ -138,7 +138,7 @@ netcp: netcp@24000000 {
 	/* NetCP address range */
 	ranges = <0 0x24000000 0x1000000>;

-	clocks = <&clkpa>, <&clkcpgmac>, <&chipclk12>;
+	clocks = <&clkpa>, <&clkcpgmac>, <&cpts_mux>;
 	clock-names = "pa_clk", "ethss_clk", "cpts";
 	dma-coherent;

@@ -162,6 +162,14 @@ netcp: netcp@24000000 {
 			cpts-ext-ts-inputs = <6>;
 			cpts-ts-comp-length;

+			cpts_mux: cpts_refclk_mux {
+				#clock-cells = <0>;
+				clocks = <&chipclk12>, <&chipclk13>;
+				cpts-mux-tbl = <0>, <1>;
+				assigned-clocks = <&cpts_mux>;
+				assigned-clock-parents = <&chipclk12>;

+			};
+
 			interfaces {
 				gbe0: interface-0 {
 					slave-port = <0>;

diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c
index 938de22..ef94316 100644
--- a/drivers/net/ethernet/ti/cpts.c
+++ b/drivers/net/ethernet/ti/cpts.c

@@ -17,6 +17,7 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301  USA
  */
+#include <linux/clk-provider.h>
 #include <linux/err.h>
 #include <linux/if.h>
 #include <linux/hrtimer.h>

@@ -672,6 +673,7 @@ int cpts_register(struct cpts *cpts)
 	cpts->phc_index = ptp_clock_index(cpts->clock);

 	schedule_delayed_work(&cpts->overflow_work, cpts->ov_check_period);
+

 	return 0;

 err_ptp:

@@ -741,6 +743,54 @@ static void cpts_calc_mult_shift(struct cpts *cpts)
 		 freq, cpts->cc_mult, cpts->cc.shift, (ns - NSEC_PER_SEC));
 }

+static int cpts_of_mux_clk_setup(struct cpts *cpts, struct device_node *node)
+{
+	unsigned int num_parents;
+	const char **parent_names;
+	struct device_node *refclk_np;
+	void __iomem *reg;
+	struct clk *clk;
+	u32 *mux_table;
+	int ret;
+
+	refclk_np = of_get_child_by_name(node, "cpts_refclk_mux");
+	if (!refclk_np)
+		return -EINVAL;
+
+	num_parents = of_clk_get_parent_count(refclk_np);
+	if (num_parents < 1) {
+		dev_err(cpts->dev, "mux-clock %s must have parents\n",
+			refclk_np->name);
+		return -EINVAL;
+	}
+	parent_names = devm_kzalloc(cpts->dev, (sizeof(char *) * num_parents),
+				    GFP_KERNEL);
+	if (!parent_names)
+		return -ENOMEM;
+
+	of_clk_parent_fill(refclk_np, parent_names, num_parents);
+
+	mux_table = devm_kzalloc(cpts->dev, sizeof(*mux_table) * (32 + 1),
+				 GFP_KERNEL);
+	if (!mux_table)
+		return -ENOMEM;
+
+	ret = of_property_read_variable_u32_array(refclk_np, "cpts-mux-tbl",
+						  mux_table, 1, 32);
+	if (ret < 0)
+		return ret;
+
+	reg = &cpts->reg->rftclk_sel;
+
+	clk = clk_register_mux_table(cpts->dev, refclk_np->name,
+				     parent_names, num_parents,
+				     0, reg, 0, 0x1F, 0, mux_table, NULL);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	return of_clk_add_provider(refclk_np, of_clk_src_simple_get, clk);