clk: renesas: Introduce and use rcar_clk_get_rate64_div_table function

Introduce new helper function to handle clock type that uses
clk_div_table struct. Based vaguely on Linux code. Make use
of clk_div_table in RPC clocks handling.

The E3/D3 RPCSRC need to be handled differently and will be addressed in
subsequence patch.

Based on Linux commit db4a0073cc82 ("clk: renesas: rcar-gen3: Add RPC
clocks") by Sergei Shtylyov.

Signed-off-by: Hai Pham <hai.pham.ud@renesas.com>
Signed-off-by: Marek Vasut <marek.vasut+renesas@mailbox.org>
Marek: - Squash patches to avoid adding unused code:
         clk: renesas: Make use of clk_div_table in RPC clocks handling
         clk: renesas: Introduce rcar_clk_get_rate64_div_table function
       - Move the new code to the beginning of clk-rcar-gen3 to avoid
         tables mixed with code
       - Use rcar_ prefix for get_table_div function
       - Get rid of custom macros, use GENMASK. Use custom field_get
         implementation as the generic FIELD_GET does not support
	 constant mask and u32_get_bits requires higher optimization level
       - Pass in the register bit mask instead of width/shift combination
       - Turn rcar_clk_get_rate64_div_table into s64, as it can return -EINVAL

2 files changed, 64 insertions, 34 deletions

diff --git a/drivers/clk/renesas/clk-rcar-gen3.c b/drivers/clk/renesas/clk-rcar-gen3.c
index 3df6aaabf0..84bd7fe8b0 100644
--- a/drivers/clk/renesas/clk-rcar-gen3.c
+++ b/drivers/clk/renesas/clk-rcar-gen3.c
@@ -20,6 +20,7 @@
 #include <wait_bit.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
+#include <linux/bitfield.h>
 #include <linux/bitops.h>
 #include <linux/clk-provider.h>
 #include <reset-uclass.h>
@@ -33,10 +34,8 @@
 #define CPG_PLL2CR		0x002c
 #define CPG_PLL4CR		0x01f4
 
-#define CPG_RPC_PREDIV_MASK	0x3
-#define CPG_RPC_PREDIV_OFFSET	3
-#define CPG_RPC_POSTDIV_MASK	0x7
-#define CPG_RPC_POSTDIV_OFFSET	0
+/* Non-constant mask variant of FIELD_GET */
+#define field_get(_mask, _reg) (((_reg) & (_mask)) >> (ffs(_mask) - 1))
 
 /*
  * SDn Clock
@@ -85,6 +84,45 @@ static const struct clk_div_table cpg_sd_div_table[] = {
 	CPG_SD_DIV_TABLE_DATA(1,        0,        4,          0,       32),
 };
 
+static const struct clk_div_table cpg_rpcsrc_div_table[] = {
+	{ 2, 5 }, { 3, 6 }, { 0, 0 },
+};
+
+static const struct clk_div_table cpg_rpc_div_table[] = {
+	{ 1, 2 }, { 3, 4 }, { 5, 6 }, { 7, 8 }, { 0, 0 },
+};
+
+static unsigned int rcar_clk_get_table_div(const struct clk_div_table *table,
+					   const u32 value)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->val == value)
+			return clkt->div;
+	return 0;
+}
+
+static __always_inline s64
+rcar_clk_get_rate64_div_table(unsigned int parent, u64 parent_rate,
+			      void __iomem *reg, const u32 mask,
+			      const struct clk_div_table *table, char *name)
+{
+	u32 value, div;
+	u64 rate;
+
+	value = field_get(mask, readl(reg));
+	div = rcar_clk_get_table_div(table, value);
+	if (!div)
+		return -EINVAL;
+
+	rate = parent_rate / div;
+	debug("%s[%i] %s clk: parent=%i div=%u => rate=%llu\n",
+	      __func__, __LINE__, name, parent, div, rate);
+
+	return rate;
+}
+
 static int gen3_clk_get_parent(struct gen3_clk_priv *priv, struct clk *clk,
 			       struct cpg_mssr_info *info, struct clk *parent)
 {
@@ -183,7 +221,7 @@ static u64 gen3_clk_get_rate64(struct clk *clk)
 	const struct cpg_core_clk *core;
 	const struct rcar_gen3_cpg_pll_config *pll_config =
 					priv->cpg_pll_config;
-	u32 value, div, prediv, postdiv;
+	u32 value, div;
 	u64 rate = 0;
 	int i, ret;
 
@@ -313,40 +351,29 @@ static u64 gen3_clk_get_rate64(struct clk *clk)
 
 		return -EINVAL;
 
+	case CLK_TYPE_GEN3_RPCSRC:
+		return rcar_clk_get_rate64_div_table(core->parent,
+						     gen3_clk_get_rate64(&parent),
+						     priv->base + CPG_RPCCKCR,
+						     CPG_RPCCKCR_DIV_POST_MASK,
+						     cpg_rpcsrc_div_table, "RPCSRC");
+
 	case CLK_TYPE_GEN3_RPC:
-	case CLK_TYPE_GEN3_RPCD2:
 	case CLK_TYPE_GEN4_RPC:
-	case CLK_TYPE_GEN4_RPCD2:
-		rate = gen3_clk_get_rate64(&parent);
-
-		value = readl(priv->base + CPG_RPCCKCR);
-
-		prediv = (value >> CPG_RPC_PREDIV_OFFSET) &
-			 CPG_RPC_PREDIV_MASK;
-		if (prediv == 2)
-			rate /= 5;
-		else if (prediv == 3)
-			rate /= 6;
-		else
-			return -EINVAL;
-
-		postdiv = (value >> CPG_RPC_POSTDIV_OFFSET) &
-			  CPG_RPC_POSTDIV_MASK;
-
-		if (postdiv % 2 != 0) {
-			rate /= postdiv + 1;
+		return rcar_clk_get_rate64_div_table(core->parent,
+						     gen3_clk_get_rate64(&parent),
+						     priv->base + CPG_RPCCKCR,
+						     CPG_RPCCKCR_DIV_PRE_MASK,
+						     cpg_rpc_div_table, "RPC");
 
-			if (core->type == CLK_TYPE_GEN3_RPCD2)
-				rate /= 2;
-
-			debug("%s[%i] RPC clk: parent=%i prediv=%i postdiv=%i => rate=%llu\n",
-			      __func__, __LINE__,
-			      core->parent, prediv, postdiv, rate);
+	case CLK_TYPE_GEN3_RPCD2:
+	case CLK_TYPE_GEN4_RPCD2:
+		rate = gen3_clk_get_rate64(&parent) / 2;
 
-			return rate;
-		}
+		debug("%s[%i] RPCD2 clk: parent=%i => rate=%llu\n",
+		      __func__, __LINE__, core->parent, rate);
 
-		return -EINVAL;
+		return rate;
 
 	}
 
diff --git a/drivers/clk/renesas/rcar-gen3-cpg.h b/drivers/clk/renesas/rcar-gen3-cpg.h
index 007610bb4d..41a30c569c 100644
--- a/drivers/clk/renesas/rcar-gen3-cpg.h
+++ b/drivers/clk/renesas/rcar-gen3-cpg.h
@@ -112,6 +112,9 @@ struct rcar_gen3_cpg_pll_config {
 #define CPG_RST_MODEMR	0x060
 
 #define CPG_RPCCKCR	0x238
+#define CPG_RPCCKCR_DIV_POST_MASK	GENMASK(4, 3)
+#define CPG_RPCCKCR_DIV_PRE_MASK	GENMASK(2, 0)
+
 #define CPG_RCKCR	0x240
 
 struct gen3_clk_priv {