[4/5] clk: eyeq5: add OSPI table-based divider clock

diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 84fe0a89b8df..63cc354f41ab 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -227,7 +227,7 @@  config COMMON_CLK_EYEQ5
 		This drivers provides the fixed clocks found on the Mobileye EyeQ5
 		SoC. Its registers live in a shared register region called OLB.
 		It provides 10 read-only PLLs derived from the main crystal clock which
-		must be constant.
+		must be constant and one divider clock based on one PLLs.
 
 config COMMON_CLK_FSL_FLEXSPI
 	tristate "Clock driver for FlexSPI on Layerscape SoCs"
diff --git a/drivers/clk/clk-eyeq5.c b/drivers/clk/clk-eyeq5.c
index 74bcb8cec5c1..3382f4d870d7 100644
--- a/drivers/clk/clk-eyeq5.c
+++ b/drivers/clk/clk-eyeq5.c
@@ -3,8 +3,9 @@ 
  * PLL clock driver for the Mobileye EyeQ5 platform.
  *
  * This controller handles 10 read-only PLLs, all derived from the same main
- * crystal clock. The parent clock is expected to be constant. This driver is
- * custom to this platform, its registers live in a shared region called OLB.
+ * crystal clock. It also exposes one divider clock, a child of one of the
+ * PLLs. The parent clock is expected to be constant. This driver is custom to
+ * this platform, its registers live in a shared region called OLB.
  *
  * We use eq5c_ as prefix, as-in "EyeQ5 Clock", but way shorter.
  *
@@ -77,6 +78,8 @@  static const struct eq5c_pll {
 	[EQ5C_PLL_DDR1] = { .name = "pll-ddr1", .reg = OLB_PCSR_DDR1(0), },
 };
 
+#define EQ5C_OSPI_DIV_CLK_NAME	"div-ospi"
+
 static int eq5c_pll_parse_registers(u32 r0, u32 r1, unsigned long *mult,
 				    unsigned long *div, unsigned long *acc)
 {
@@ -131,6 +134,128 @@  static int eq5c_pll_parse_registers(u32 r0, u32 r1, unsigned long *mult,
 	return 0;
 }
 
+#define OLB_OSPI_REG		0x11C
+#define OLB_OSPI_DIV_MASK	GENMASK(3, 0)
+#define OLB_OSPI_DIV_MASK_WIDTH	4
+
+static const struct clk_div_table eq5c_ospi_div_table[] = {
+	{ .val = 0, .div = 2 },
+	{ .val = 1, .div = 4 },
+	{ .val = 2, .div = 6 },
+	{ .val = 3, .div = 8 },
+	{ .val = 4, .div = 10 },
+	{ .val = 5, .div = 12 },
+	{ .val = 6, .div = 14 },
+	{ .val = 7, .div = 16 },
+	{} /* sentinel */
+};
+
+struct eq5c_ospi_div {
+	struct clk_hw	hw;
+	struct regmap	*olb;
+};
+
+static struct eq5c_ospi_div *clk_hw_to_ospi_priv(struct clk_hw *hw)
+{
+	return container_of(hw, struct eq5c_ospi_div, hw);
+}
+
+static unsigned long eq5c_ospi_div_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct eq5c_ospi_div *div = clk_hw_to_ospi_priv(hw);
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(div->olb, OLB_OSPI_REG, &val);
+
+	if (ret) {
+		pr_err("%s: regmap_read failed: %d\n", __func__, ret);
+		return 0;
+	}
+
+	val = FIELD_GET(OLB_OSPI_DIV_MASK, val);
+
+	return divider_recalc_rate(hw, parent_rate, val,
+				   eq5c_ospi_div_table, 0,
+				   OLB_OSPI_DIV_MASK_WIDTH);
+}
+
+static long eq5c_ospi_div_round_rate(struct clk_hw *hw,
+				     unsigned long rate, unsigned long *prate)
+{
+	return divider_round_rate(hw, rate, prate, eq5c_ospi_div_table,
+				  OLB_OSPI_DIV_MASK_WIDTH, 0);
+}
+
+static int eq5c_ospi_div_determine_rate(struct clk_hw *hw,
+					struct clk_rate_request *req)
+{
+	return divider_determine_rate(hw, req, eq5c_ospi_div_table,
+				      OLB_OSPI_DIV_MASK_WIDTH, 0);
+}
+
+static int eq5c_ospi_div_set_rate(struct clk_hw *hw,
+				  unsigned long rate, unsigned long parent_rate)
+{
+	struct eq5c_ospi_div *div = clk_hw_to_ospi_priv(hw);
+	unsigned int val;
+	int value, ret;
+
+	value = divider_get_val(rate, parent_rate, eq5c_ospi_div_table,
+				OLB_OSPI_DIV_MASK_WIDTH, 0);
+	if (value < 0)
+		return value;
+
+	ret = regmap_read(div->olb, OLB_OSPI_REG, &val);
+	if (ret) {
+		pr_err("%s: regmap_read failed: %d\n", __func__, ret);
+		return -ret;
+	}
+
+	val &= ~OLB_OSPI_DIV_MASK;
+	val |= FIELD_PREP(OLB_OSPI_DIV_MASK, value);
+
+	ret = regmap_write(div->olb, OLB_OSPI_REG, val);
+	if (ret) {
+		pr_err("%s: regmap_write failed: %d\n", __func__, ret);
+		return -ret;
+	}
+
+	return 0;
+}
+
+const struct clk_ops eq5c_ospi_div_ops = {
+	.recalc_rate = eq5c_ospi_div_recalc_rate,
+	.round_rate = eq5c_ospi_div_round_rate,
+	.determine_rate = eq5c_ospi_div_determine_rate,
+	.set_rate = eq5c_ospi_div_set_rate,
+};
+
+static struct clk_hw *eq5c_init_ospi_div(const struct clk_hw *parent,
+					 struct regmap *olb)
+{
+	struct eq5c_ospi_div *div;
+	int ret;
+
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		return ERR_PTR(-ENOENT);
+
+	div->olb = olb;
+	div->hw.init = CLK_HW_INIT_HW(EQ5C_OSPI_DIV_CLK_NAME, parent,
+				      &eq5c_ospi_div_ops, 0);
+
+	ret = clk_hw_register(NULL, &div->hw);
+	if (ret) {
+		pr_err("failed registering div_ospi: %d\n", ret);
+		kfree(div);
+		return ERR_PTR(-ENOENT);
+	}
+
+	return &div->hw;
+}
+
 static void eq5c_init(struct device_node *np)
 {
 	struct device_node *parent_np = of_get_parent(np);
@@ -139,13 +264,15 @@  static void eq5c_init(struct device_node *np)
 	struct clk_hw *parent_clk_hw;
 	struct clk *parent_clk;
 	struct regmap *olb;
+	size_t nb_clks;
 	int i;
 
-	data = kzalloc(struct_size(data, hws, ARRAY_SIZE(eq5c_plls)), GFP_KERNEL);
+	nb_clks = ARRAY_SIZE(eq5c_plls) + 1;
+	data = kzalloc(struct_size(data, hws, nb_clks), GFP_KERNEL);
 	if (!data)
 		return;
 
-	data->num = ARRAY_SIZE(eq5c_plls);
+	data->num = nb_clks;
 
 	/*
 	 * TODO: currently, if OLB is not available, we log an error and early
@@ -205,6 +332,14 @@  static void eq5c_init(struct device_node *np)
 		}
 	}
 
+	/*
+	 * Register the OSPI table-based divider clock manually. This is
+	 * equivalent to drivers/clk/clk-divider.c, but using regmap to access
+	 * its register.
+	 */
+	i = ARRAY_SIZE(eq5c_plls);
+	data->hws[i] = eq5c_init_ospi_div(data->hws[EQ5C_PLL_PER], olb);
+
 	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, data);
 }