2 files changed, 49 insertions, 0 deletions

diff --git a/sys/dev/extres/clk/clk_div.c b/sys/dev/extres/clk/clk_div.c
index 51cc838303a5..bf663f28c3a4 100644
--- a/sys/dev/extres/clk/clk_div.c
+++ b/sys/dev/extres/clk/clk_div.c
@@ -68,6 +68,8 @@ struct clknode_div_sc {
 	uint32_t	f_width;
 	int		div_flags;
 	uint32_t	divider;	/* in natural form */
+
+	struct clk_div_table	*div_table;
 };
 
 static clknode_method_t clknode_div_methods[] = {
@@ -80,6 +82,36 @@ static clknode_method_t clknode_div_methods[] = {
 DEFINE_CLASS_1(clknode_div, clknode_div_class, clknode_div_methods,
    sizeof(struct clknode_div_sc), clknode_class);
 
+static uint32_t
+clknode_div_table_get_divider(struct clknode_div_sc *sc, uint32_t divider)
+{
+	struct clk_div_table *table;
+
+	if (!(sc->div_flags & CLK_DIV_WITH_TABLE))
+		return (divider);
+
+	for (table = sc->div_table; table->divider != 0; table++)
+		if (table->value == sc->divider)
+			return (table->divider);
+
+	return (0);
+}
+
+static uint32_t
+clknode_div_table_get_value(struct clknode_div_sc *sc, uint32_t divider)
+{
+	struct clk_div_table *table;
+
+	if (!(sc->div_flags & CLK_DIV_WITH_TABLE))
+		return (divider);
+
+	for (table = sc->div_table; table->divider != 0; table++)
+		if (table->divider == sc->divider)
+			return (table->value);
+
+	return (0);
+}
+
 static int
 clknode_div_init(struct clknode *clk, device_t dev)
 {
@@ -101,6 +133,11 @@ clknode_div_init(struct clknode *clk, device_t dev)
 		i_div++;
 	f_div = (reg >> sc->f_shift) & sc->f_mask;
 	sc->divider = i_div << sc->f_width | f_div;
+
+	sc->divider = clknode_div_table_get_divider(sc, sc->divider);
+	if (sc->divider == 0)
+		panic("%s: divider is zero!\n", clknode_get_name(clk));
+
 	clknode_init_parent_idx(clk, 0);
 	return(0);
 }
@@ -177,6 +214,10 @@ clknode_div_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
 		    (*fout != (_fin / divider)))
 			return (ERANGE);
 
+		divider = clknode_div_table_get_value(sc, divider);
+		if (divider == 0)
+			return (ERANGE);
+
 		DEVICE_LOCK(clk);
 		rv = MD4(clk, sc->offset,
 		    (sc->i_mask << sc->i_shift) | (sc->f_mask << sc->f_shift),
@@ -214,6 +255,7 @@ clknode_div_register(struct clkdom *clkdom, struct clk_div_def *clkdef)
 	sc->f_width = clkdef->f_width;
 	sc->f_mask = (1 << clkdef->f_width) - 1;
 	sc->div_flags = clkdef->div_flags;
+	sc->div_table = clkdef->div_table;
 
 	clknode_register(clkdom, clk);
 	return (0);
diff --git a/sys/dev/extres/clk/clk_div.h b/sys/dev/extres/clk/clk_div.h
index 85b8a0194eb4..4d5f7fa3008b 100644
--- a/sys/dev/extres/clk/clk_div.h
+++ b/sys/dev/extres/clk/clk_div.h
@@ -32,6 +32,12 @@
 #include <dev/extres/clk/clk.h>
 
 #define	CLK_DIV_ZERO_BASED	0x0001 /* Zero based divider. */
+#define	CLK_DIV_WITH_TABLE	0x0002 /* Table to lookup the real value */
+
+struct clk_div_table {
+	uint32_t	value;
+	uint32_t	divider;
+};
 
 struct clk_div_def {
 	struct clknode_init_def clkdef;
@@ -41,6 +47,7 @@ struct clk_div_def {
 	uint32_t		f_shift;	/* Fractional divide bits, */
 	uint32_t		f_width;	/* set to 0 for int divider */
 	int			div_flags;	/* Divider-specific flags */
+	struct clk_div_table	*div_table;	/* Divider table */
 };
 
 int clknode_div_register(struct clkdom *clkdom, struct clk_div_def *clkdef);