i2c: cdns: Support different bus speeds

400kHz is maximum freq which can be used on Xilinx ZynqMP.
Support it with standard divider calculator.
Input freq is hardcoded to 100MHz input freq till we have clock driver
which can provide this information for exact configuration.

Signed-off-by: Michal Simek <michal.simek@xilinx.com>
Reviewed-by: Heiko Schocher <hs@denx.de>

drivers/i2c/i2c-cdns.c

@@ -112,6 +112,7 @@ static void cdns_i2c_debug_status(struct cdns_i2c_regs *cdns_i2c)
 
 struct i2c_cdns_bus {
 	int id;
+	unsigned int input_freq;
 	struct cdns_i2c_regs __iomem *regs;	/* register base */
 };
 
@@ -133,20 +134,79 @@ static u32 cdns_i2c_wait(struct cdns_i2c_regs *cdns_i2c, u32 mask)
 	return int_status & mask;
 }
 
+#define CDNS_I2C_DIVA_MAX	4
+#define CDNS_I2C_DIVB_MAX	64
+
+static int cdns_i2c_calc_divs(unsigned long *f, unsigned long input_clk,
+		unsigned int *a, unsigned int *b)
+{
+	unsigned long fscl = *f, best_fscl = *f, actual_fscl, temp;
+	unsigned int div_a, div_b, calc_div_a = 0, calc_div_b = 0;
+	unsigned int last_error, current_error;
+
+	/* calculate (divisor_a+1) x (divisor_b+1) */
+	temp = input_clk / (22 * fscl);
+
+	/*
+	 * If the calculated value is negative or 0CDNS_I2C_DIVA_MAX,
+	 * the fscl input is out of range. Return error.
+	 */
+	if (!temp || (temp > (CDNS_I2C_DIVA_MAX * CDNS_I2C_DIVB_MAX)))
+		return -EINVAL;
+
+	last_error = -1;
+	for (div_a = 0; div_a < CDNS_I2C_DIVA_MAX; div_a++) {
+		div_b = DIV_ROUND_UP(input_clk, 22 * fscl * (div_a + 1));
+
+		if ((div_b < 1) || (div_b > CDNS_I2C_DIVB_MAX))
+			continue;
+		div_b--;
+
+		actual_fscl = input_clk / (22 * (div_a + 1) * (div_b + 1));
+
+		if (actual_fscl > fscl)
+			continue;
+
+		current_error = ((actual_fscl > fscl) ? (actual_fscl - fscl) :
+							(fscl - actual_fscl));
+
+		if (last_error > current_error) {
+			calc_div_a = div_a;
+			calc_div_b = div_b;
+			best_fscl = actual_fscl;
+			last_error = current_error;
+		}
+	}
+
+	*a = calc_div_a;
+	*b = calc_div_b;
+	*f = best_fscl;
+
+	return 0;
+}
+
 static int cdns_i2c_set_bus_speed(struct udevice *dev, unsigned int speed)
 {
 	struct i2c_cdns_bus *bus = dev_get_priv(dev);
+	u32 div_a = 0, div_b = 0;
+	unsigned long speed_p = speed;
+	int ret = 0;
 
-	if (speed != 100000) {
-		printf("%s, failed to set clock speed to %u\n", __func__,
-		       speed);
+	if (speed > 400000) {
+		debug("%s, failed to set clock speed to %u\n", __func__,
+		      speed);
 		return -EINVAL;
 	}
 
-	/* TODO: Calculate dividers based on CPU_CLK_1X */
-	/* 111MHz / ( (3 * 17) * 22 ) = ~100KHz */
-	writel((16 << CDNS_I2C_CONTROL_DIV_B_SHIFT) |
-		(2 << CDNS_I2C_CONTROL_DIV_A_SHIFT), &bus->regs->control);
+	ret = cdns_i2c_calc_divs(&speed_p, bus->input_freq, &div_a, &div_b);
+	if (ret)
+		return ret;
+
+	debug("%s: div_a: %d, div_b: %d, input freq: %d, speed: %d/%ld\n",
+	      __func__, div_a, div_b, bus->input_freq, speed, speed_p);
+
+	writel((div_b << CDNS_I2C_CONTROL_DIV_B_SHIFT) |
+	       (div_a << CDNS_I2C_CONTROL_DIV_A_SHIFT), &bus->regs->control);
 
 	/* Enable master mode, ack, and 7-bit addressing */
 	setbits_le32(&bus->regs->control, CDNS_I2C_CONTROL_MS |
@@ -293,6 +353,8 @@ static int cdns_i2c_ofdata_to_platdata(struct udevice *dev)
 	if (!i2c_bus->regs)
 		return -ENOMEM;
 
+	i2c_bus->input_freq = 100000000; /* TODO hardcode input freq for now */
+
 	return 0;
 }