[PATCH v2 07/14] clk: rockchip: rk3308: Add support for SCLK_RTC32K clock

[Jonas Karlman]

From: Finley Xiao <finley.xiao at rock-chips.com>

Add support to get and set the SCLK_RTC32K clock rate.

Signed-off-by: Finley Xiao <finley.xiao at rock-chips.com>
[jonas at kwiboo.se: Update commit message]
Signed-off-by: Jonas Karlman <jonas at kwiboo.se>
---
v2: No change
---
 arch/arm/include/asm/arch-rk3308/cru_rk3308.h | 14 +++
 drivers/clk/rockchip/clk_rk3308.c             | 95 +++++++++++++++++++
 2 files changed, 109 insertions(+)

diff --git a/arch/arm/include/asm/arch-rk3308/cru_rk3308.h b/arch/arm/include/asm/arch-rk3308/cru_rk3308.h
index 84b63e4d5682..091ae82d7cc1 100644
--- a/arch/arm/include/asm/arch-rk3308/cru_rk3308.h
+++ b/arch/arm/include/asm/arch-rk3308/cru_rk3308.h
@@ -147,6 +147,20 @@ enum {
 	CORE_DIV_CON_SHIFT	= 0,
 	CORE_DIV_CON_MASK	= 0x0f << CORE_DIV_CON_SHIFT,
 
+	/* CRU_CLK_SEL2_CON */
+	CLK_RTC32K_SEL_SHIFT	= 8,
+	CLK_RTC32K_SEL_MASK	= 3 << CLK_RTC32K_SEL_SHIFT,
+	CLK_RTC32K_IO		= 0,
+	CLK_RTC32K_PVTM,
+	CLK_RTC32K_FRAC_DIV,
+	CLK_RTC32K_DIV,
+
+	/* CRU_CLK_SEL3_CON */
+	CLK_RTC32K_FRAC_NUMERATOR_SHIFT		= 16,
+	CLK_RTC32K_FRAC_NUMERATOR_MASK		= 0xffff << 16,
+	CLK_RTC32K_FRAC_DENOMINATOR_SHIFT	= 0,
+	CLK_RTC32K_FRAC_DENOMINATOR_MASK	= 0xffff,
+
 	/* CRU_CLK_SEL5_CON */
 	BUS_PLL_SEL_SHIFT	= 6,
 	BUS_PLL_SEL_MASK	= 0x3 << BUS_PLL_SEL_SHIFT,
diff --git a/drivers/clk/rockchip/clk_rk3308.c b/drivers/clk/rockchip/clk_rk3308.c
index 7755b0161118..7515fc8bb244 100644
--- a/drivers/clk/rockchip/clk_rk3308.c
+++ b/drivers/clk/rockchip/clk_rk3308.c
@@ -65,6 +65,57 @@ static struct rockchip_pll_clock rk3308_pll_clks[] = {
 		      RK3308_MODE_CON, 6, 10, 0, NULL),
 };
 
+/*
+ *
+ * rational_best_approximation(31415, 10000,
+ *		(1 << 8) - 1, (1 << 5) - 1, &n, &d);
+ *
+ * you may look at given_numerator as a fixed point number,
+ * with the fractional part size described in given_denominator.
+ *
+ * for theoretical background, see:
+ * http://en.wikipedia.org/wiki/Continued_fraction
+ */
+static void rational_best_approximation(unsigned long given_numerator,
+					unsigned long given_denominator,
+					unsigned long max_numerator,
+					unsigned long max_denominator,
+					unsigned long *best_numerator,
+					unsigned long *best_denominator)
+{
+	unsigned long n, d, n0, d0, n1, d1;
+
+	n = given_numerator;
+	d = given_denominator;
+	n0 = 0;
+	d1 = 0;
+	n1 = 1;
+	d0 = 1;
+	for (;;) {
+		unsigned long t, a;
+
+		if (n1 > max_numerator || d1 > max_denominator) {
+			n1 = n0;
+			d1 = d0;
+			break;
+		}
+		if (d == 0)
+			break;
+		t = d;
+		a = n / d;
+		d = n % d;
+		n = t;
+		t = n0 + a * n1;
+		n0 = n1;
+		n1 = t;
+		t = d0 + a * d1;
+		d0 = d1;
+		d1 = t;
+	}
+	*best_numerator = n1;
+	*best_denominator = d1;
+}
+
 static ulong rk3308_armclk_set_clk(struct rk3308_clk_priv *priv, ulong hz)
 {
 	struct rk3308_cru *cru = priv->cru;
@@ -832,6 +883,44 @@ static ulong rk3308_crypto_set_clk(struct rk3308_clk_priv *priv, ulong clk_id,
 	return rk3308_crypto_get_clk(priv, clk_id);
 }
 
+static ulong rk3308_rtc32k_get_clk(struct rk3308_clk_priv *priv, ulong clk_id)
+{
+	struct rk3308_cru *cru = priv->cru;
+	unsigned long m, n;
+	u32 con, fracdiv;
+
+	con = readl(&cru->clksel_con[2]);
+	if ((con & CLK_RTC32K_SEL_MASK) >> CLK_RTC32K_SEL_SHIFT !=
+	    CLK_RTC32K_FRAC_DIV)
+		return -EINVAL;
+
+	fracdiv = readl(&cru->clksel_con[3]);
+	m = fracdiv & CLK_RTC32K_FRAC_NUMERATOR_MASK;
+	m >>= CLK_RTC32K_FRAC_NUMERATOR_SHIFT;
+	n = fracdiv & CLK_RTC32K_FRAC_DENOMINATOR_MASK;
+	n >>= CLK_RTC32K_FRAC_DENOMINATOR_SHIFT;
+
+	return OSC_HZ * m / n;
+}
+
+static ulong rk3308_rtc32k_set_clk(struct rk3308_clk_priv *priv, ulong clk_id,
+				   ulong hz)
+{
+	struct rk3308_cru *cru = priv->cru;
+	unsigned long m, n, val;
+
+	rational_best_approximation(hz, OSC_HZ,
+				    GENMASK(16 - 1, 0),
+				    GENMASK(16 - 1, 0),
+				    &m, &n);
+	val = m << CLK_RTC32K_FRAC_NUMERATOR_SHIFT | n;
+	writel(val, &cru->clksel_con[3]);
+	rk_clrsetreg(&cru->clksel_con[2], CLK_RTC32K_SEL_MASK,
+		     CLK_RTC32K_FRAC_DIV << CLK_RTC32K_SEL_SHIFT);
+
+	return rk3308_rtc32k_get_clk(priv, clk_id);
+}
+
 static ulong rk3308_clk_get_rate(struct clk *clk)
 {
 	struct rk3308_clk_priv *priv = dev_get_priv(clk->dev);
@@ -912,6 +1001,9 @@ static ulong rk3308_clk_get_rate(struct clk *clk)
 	case SCLK_CRYPTO_APK:
 		rate = rk3308_crypto_get_clk(priv, clk->id);
 		break;
+	case SCLK_RTC32K:
+		rate = rk3308_rtc32k_get_clk(priv, clk->id);
+		break;
 	default:
 		return -ENOENT;
 	}
@@ -990,6 +1082,9 @@ static ulong rk3308_clk_set_rate(struct clk *clk, ulong rate)
 	case SCLK_CRYPTO_APK:
 		ret = rk3308_crypto_set_clk(priv, clk->id, rate);
 		break;
+	case SCLK_RTC32K:
+		ret = rk3308_rtc32k_set_clk(priv, clk->id, rate);
+		break;
 	default:
 		return -ENOENT;
 	}
-- 
2.43.2