soc/mediatek: move i2c function to common folder

Move mtk_i2c_max_step_cnt, mtk_i2c_check_ac_timing, mtk_i2c_speed_init
and mtk_i2c_calculate_speed to common folder to share with MT8186.

TEST=test on tomato ok
TEST=emerge-asurada coreboot
BUG=b:202871018

Signed-off-by: Rex-BC Chen <rex-bc.chen@mediatek.com>
Change-Id: I4a702741c763bf9261cea90d0d71c08b6e28c261
Reviewed-on: https://review.coreboot.org/c/coreboot/+/59295
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Yu-Ping Wu <yupingso@google.com>

10 files changed, 297 insertions, 258 deletions

diff --git a/src/soc/mediatek/common/i2c.c b/src/soc/mediatek/common/i2c.c
index 1e713dbea6..b9b02cd7fd 100644
--- a/src/soc/mediatek/common/i2c.c
+++ b/src/soc/mediatek/common/i2c.c
@@ -8,6 +8,7 @@
 #include <symbols.h>
 #include <device/mmio.h>
 #include <soc/i2c.h>
+#include <soc/i2c_common.h>
 #include <device/i2c_simple.h>
 
 const struct i2c_spec_values standard_mode_spec = {
@@ -31,7 +32,15 @@ const struct i2c_spec_values fast_mode_plus_spec = {
 	.min_su_dat_ns = 50 + I2C_FAST_MODE_PLUS_BUFFER,
 };
 
-__weak void mtk_i2c_dump_more_info(struct mt_i2c_regs *regs) { /* do nothing */ }
+__weak void mtk_i2c_dump_more_info(struct mt_i2c_regs *regs)
+{
+	/* do nothing */
+}
+
+__weak void mtk_i2c_config_timing(struct mt_i2c_regs *regs, struct mtk_i2c *bus_ctrl)
+{
+	/* do nothing */
+}
 
 const struct i2c_spec_values *mtk_i2c_get_spec(uint32_t speed)
 {
@@ -281,6 +290,14 @@ static bool mtk_i2c_should_combine(struct i2c_msg *seg, int left_count)
 	    seg[0].slave == seg[1].slave);
 }
 
+static int mtk_i2c_max_step_cnt(uint32_t target_speed)
+{
+	if (target_speed > I2C_SPEED_FAST_PLUS)
+		return MAX_HS_STEP_CNT_DIV;
+	else
+		return MAX_STEP_CNT_DIV;
+}
+
 int platform_i2c_transfer(unsigned int bus, struct i2c_msg *segments,
 			  int seg_count)
 {
@@ -307,3 +324,230 @@ int platform_i2c_transfer(unsigned int bus, struct i2c_msg *segments,
 
 	return ret;
 }
+
+/*
+ * Check and calculate i2c ac-timing.
+ *
+ * Hardware design:
+ * sample_ns = (1000000000 * (sample_cnt + 1)) / clk_src
+ * xxx_cnt_div =  spec->min_xxx_ns / sample_ns
+ *
+ * The calculation of sample_ns is rounded down;
+ * otherwise xxx_cnt_div would be greater than the smallest spec.
+ * The sda_timing is chosen as the middle value between
+ * the largest and smallest.
+ */
+int mtk_i2c_check_ac_timing(uint8_t bus, uint32_t clk_src,
+			    uint32_t check_speed,
+			    uint32_t step_cnt,
+			    uint32_t sample_cnt)
+{
+	const struct i2c_spec_values *spec;
+	uint32_t su_sta_cnt, low_cnt, high_cnt, max_step_cnt;
+	uint32_t sda_max, sda_min, clk_ns, max_sta_cnt = 0x100;
+	uint32_t sample_ns = ((uint64_t)NSECS_PER_SEC * (sample_cnt + 1)) / clk_src;
+	struct mtk_i2c_ac_timing *ac_timing;
+
+	spec = mtk_i2c_get_spec(check_speed);
+
+	clk_ns = NSECS_PER_SEC / clk_src;
+
+	su_sta_cnt = DIV_ROUND_UP(spec->min_su_sta_ns, clk_ns);
+	if (su_sta_cnt > max_sta_cnt)
+		return -1;
+
+	low_cnt = DIV_ROUND_UP(spec->min_low_ns, sample_ns);
+	max_step_cnt = mtk_i2c_max_step_cnt(check_speed);
+	if (2 * step_cnt > low_cnt && low_cnt < max_step_cnt) {
+		if (low_cnt > step_cnt) {
+			high_cnt = 2 * step_cnt - low_cnt;
+		} else {
+			high_cnt = step_cnt;
+			low_cnt = step_cnt;
+		}
+	} else {
+		return -2;
+	}
+
+	sda_max = spec->max_hd_dat_ns / sample_ns;
+	if (sda_max > low_cnt)
+		sda_max = 0;
+
+	sda_min = DIV_ROUND_UP(spec->min_su_dat_ns, sample_ns);
+	if (sda_min < low_cnt)
+		sda_min = 0;
+
+	if (sda_min > sda_max)
+		return -3;
+
+	ac_timing = &mtk_i2c_bus_controller[bus].ac_timing;
+	if (check_speed > I2C_SPEED_FAST_PLUS) {
+		ac_timing->hs = I2C_TIME_DEFAULT_VALUE | (sample_cnt << 12) | (high_cnt << 8);
+		ac_timing->ltiming &= ~GENMASK(15, 9);
+		ac_timing->ltiming |= (sample_cnt << 12) | (low_cnt << 9);
+		ac_timing->ext &= ~GENMASK(7, 1);
+		ac_timing->ext |= (su_sta_cnt << 1) | (1 << 0);
+	} else {
+		ac_timing->htiming = (sample_cnt << 8) | (high_cnt);
+		ac_timing->ltiming = (sample_cnt << 6) | (low_cnt);
+		ac_timing->ext = (su_sta_cnt << 8) | (1 << 0);
+	}
+
+	return 0;
+}
+
+/*
+ * Calculate i2c port speed.
+ *
+ * Hardware design:
+ * i2c_bus_freq = parent_clk / (clock_div * 2 * sample_cnt * step_cnt)
+ * clock_div: fixed in hardware, but may be various in different SoCs
+ *
+ * To calculate sample_cnt and step_cnt, we pick the highest bus frequency
+ * that is still no larger than i2c->speed_hz.
+ */
+int mtk_i2c_calculate_speed(uint8_t bus, uint32_t clk_src,
+			    uint32_t target_speed,
+			    uint32_t *timing_step_cnt,
+			    uint32_t *timing_sample_cnt)
+{
+	uint32_t step_cnt;
+	uint32_t sample_cnt;
+	uint32_t max_step_cnt;
+	uint32_t base_sample_cnt = MAX_SAMPLE_CNT_DIV;
+	uint32_t base_step_cnt;
+	uint32_t opt_div;
+	uint32_t best_mul;
+	uint32_t cnt_mul;
+	uint32_t clk_div = mtk_i2c_bus_controller[bus].ac_timing.inter_clk_div;
+	int32_t clock_div_constraint = 0;
+	int success = 0;
+
+	if (target_speed > I2C_SPEED_HIGH)
+		target_speed = I2C_SPEED_HIGH;
+
+	max_step_cnt = mtk_i2c_max_step_cnt(target_speed);
+	base_step_cnt = max_step_cnt;
+
+	/* Find the best combination */
+	opt_div = DIV_ROUND_UP(clk_src >> 1, target_speed);
+	best_mul = MAX_SAMPLE_CNT_DIV * max_step_cnt;
+
+	/* Search for the best pair (sample_cnt, step_cnt) with
+	 * 0 < sample_cnt < MAX_SAMPLE_CNT_DIV
+	 * 0 < step_cnt < max_step_cnt
+	 * sample_cnt * step_cnt >= opt_div
+	 * optimizing for sample_cnt * step_cnt being minimal
+	 */
+	for (sample_cnt = 1; sample_cnt <= MAX_SAMPLE_CNT_DIV; sample_cnt++) {
+		if (sample_cnt == 1) {
+			if (clk_div != 0)
+				clock_div_constraint = 1;
+			else
+				clock_div_constraint = 0;
+		} else {
+			if (clk_div > 1)
+				clock_div_constraint = 1;
+			else if (clk_div == 0)
+				clock_div_constraint = -1;
+			else
+				clock_div_constraint = 0;
+		}
+
+		step_cnt = DIV_ROUND_UP(opt_div + clock_div_constraint, sample_cnt);
+		if (step_cnt > max_step_cnt)
+			continue;
+
+		cnt_mul = step_cnt * sample_cnt;
+		if (cnt_mul >= best_mul)
+			continue;
+
+		if (mtk_i2c_check_ac_timing(bus, clk_src,
+					    target_speed, step_cnt - 1,
+					    sample_cnt - 1))
+			continue;
+
+		success = 1;
+		best_mul = cnt_mul;
+		base_sample_cnt = sample_cnt;
+		base_step_cnt = step_cnt;
+		if (best_mul == opt_div + clock_div_constraint)
+			break;
+
+	}
+
+	if (!success)
+		return -1;
+
+	sample_cnt = base_sample_cnt;
+	step_cnt = base_step_cnt;
+
+	if (clk_src / (2 * (sample_cnt * step_cnt - clock_div_constraint)) >
+	    target_speed)
+		return -1;
+
+	*timing_step_cnt = step_cnt - 1;
+	*timing_sample_cnt = sample_cnt - 1;
+
+	return 0;
+}
+
+void mtk_i2c_speed_init(uint8_t bus, uint32_t speed)
+{
+	uint32_t max_clk_div = MAX_CLOCK_DIV;
+	uint32_t clk_src, clk_div, step_cnt, sample_cnt;
+	uint32_t l_step_cnt, l_sample_cnt;
+	uint32_t timing_reg_value, ltiming_reg_value;
+	struct mtk_i2c *bus_ctrl;
+
+	if (bus >= I2C_BUS_NUMBER) {
+		printk(BIOS_ERR, "%s, error bus num:%d\n", __func__, bus);
+		return;
+	}
+
+	bus_ctrl = &mtk_i2c_bus_controller[bus];
+
+	for (clk_div = 1; clk_div <= max_clk_div; clk_div++) {
+		clk_src = I2C_CLK_HZ / clk_div;
+		bus_ctrl->ac_timing.inter_clk_div = clk_div - 1;
+
+		if (speed > I2C_SPEED_FAST_PLUS) {
+			/* Set master code speed register */
+			if (mtk_i2c_calculate_speed(bus, clk_src, I2C_SPEED_FAST,
+						    &l_step_cnt, &l_sample_cnt))
+				continue;
+
+			timing_reg_value = (l_sample_cnt << 8) | l_step_cnt;
+
+			/* Set the high speed mode register */
+			if (mtk_i2c_calculate_speed(bus, clk_src, speed,
+						    &step_cnt, &sample_cnt))
+				continue;
+
+			ltiming_reg_value = (l_sample_cnt << 6) | l_step_cnt |
+					    (sample_cnt << 12) | (step_cnt << 9);
+			bus_ctrl->ac_timing.inter_clk_div = (clk_div - 1) << 8 | (clk_div - 1);
+		} else {
+			if (mtk_i2c_calculate_speed(bus, clk_src, speed,
+						    &l_step_cnt, &l_sample_cnt))
+				continue;
+
+			timing_reg_value = (l_sample_cnt << 8) | l_step_cnt;
+
+			/* Disable the high speed transaction */
+			bus_ctrl->ac_timing.hs = I2C_TIME_CLR_VALUE;
+
+			ltiming_reg_value = (l_sample_cnt << 6) | l_step_cnt;
+		}
+
+		break;
+	}
+
+	if (clk_div > max_clk_div) {
+		printk(BIOS_ERR, "%s, cannot support %d hz on i2c-%d\n", __func__, speed, bus);
+		return;
+	}
+
+	/* Init i2c bus timing register. */
+	mtk_i2c_config_timing(bus_ctrl->i2c_regs, bus_ctrl);
+}
diff --git a/src/soc/mediatek/common/include/soc/i2c_common.h b/src/soc/mediatek/common/include/soc/i2c_common.h
index 72ec46a093..7df1ac5608 100644
--- a/src/soc/mediatek/common/include/soc/i2c_common.h
+++ b/src/soc/mediatek/common/include/soc/i2c_common.h
@@ -143,4 +143,15 @@ extern struct mtk_i2c mtk_i2c_bus_controller[];
 const struct i2c_spec_values *mtk_i2c_get_spec(uint32_t speed);
 void mtk_i2c_dump_more_info(struct mt_i2c_regs *regs);
 
+int mtk_i2c_check_ac_timing(uint8_t bus, uint32_t clk_src,
+			    uint32_t check_speed,
+			    uint32_t step_cnt,
+			    uint32_t sample_cnt);
+int mtk_i2c_calculate_speed(uint8_t bus, uint32_t clk_src,
+			    uint32_t target_speed,
+			    uint32_t *timing_step_cnt,
+			    uint32_t *timing_sample_cnt);
+void mtk_i2c_speed_init(uint8_t bus, uint32_t speed);
+void mtk_i2c_config_timing(struct mt_i2c_regs *regs, struct mtk_i2c *bus_ctrl);
+
 #endif
diff --git a/src/soc/mediatek/mt8173/i2c.c b/src/soc/mediatek/mt8173/i2c.c
index ff803c3d3b..dd4629d909 100644
--- a/src/soc/mediatek/mt8173/i2c.c
+++ b/src/soc/mediatek/mt8173/i2c.c
@@ -6,9 +6,6 @@
 #include <device/mmio.h>
 #include <soc/addressmap.h>
 #include <soc/i2c.h>
-#include <soc/pll.h>
-
-#define I2C_CLK_HZ (AXI_HZ / 16)
 
 struct mtk_i2c mtk_i2c_bus_controller[7] = {
 	/* i2c0 setting */
@@ -52,6 +49,9 @@ struct mtk_i2c mtk_i2c_bus_controller[7] = {
 	}
 };
 
+_Static_assert(ARRAY_SIZE(mtk_i2c_bus_controller) == I2C_BUS_NUMBER,
+	       "Wrong size of mtk_i2c_bus_controller");
+
 #define I2CTAG                "[I2C][PL] "
 
 #if CONFIG(DEBUG_I2C)
diff --git a/src/soc/mediatek/mt8173/include/soc/i2c.h b/src/soc/mediatek/mt8173/include/soc/i2c.h
index e6f0140367..02a21817d9 100644
--- a/src/soc/mediatek/mt8173/include/soc/i2c.h
+++ b/src/soc/mediatek/mt8173/include/soc/i2c.h
@@ -4,6 +4,7 @@
 #define SOC_MEDIATEK_MT8173_I2C_H
 
 #include <soc/i2c_common.h>
+#include <soc/pll.h>
 
 /* I2C Register */
 struct mt_i2c_regs {
@@ -35,6 +36,10 @@ struct mt_i2c_regs {
 	uint32_t transfer_aux_len;
 };
 
+#define I2C_CLK_HZ (AXI_HZ / 16)
+#define I2C_BUS_NUMBER 7
+#define MAX_CLOCK_DIV  32
+
 check_member(mt_i2c_regs, debug_stat, 0x64);
 
 void mtk_i2c_bus_init(uint8_t bus);
diff --git a/src/soc/mediatek/mt8183/i2c.c b/src/soc/mediatek/mt8183/i2c.c
index 3e53a72c5e..193f4b912b 100644
--- a/src/soc/mediatek/mt8183/i2c.c
+++ b/src/soc/mediatek/mt8183/i2c.c
@@ -2,12 +2,9 @@
 
 #include <assert.h>
 #include <device/mmio.h>
-#include <soc/pll.h>
 #include <soc/i2c.h>
 #include <soc/gpio.h>
 
-#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
-
 struct mtk_i2c mtk_i2c_bus_controller[] = {
 	/* i2c0 setting */
 	{
@@ -52,7 +49,8 @@ struct mtk_i2c mtk_i2c_bus_controller[] = {
 	},
 };
 
-#define I2C_BUS_NUMBER ARRAY_SIZE(mtk_i2c_bus_controller)
+_Static_assert(ARRAY_SIZE(mtk_i2c_bus_controller) == I2C_BUS_NUMBER,
+	       "Wrong size of mtk_i2c_bus_controller");
 
 struct pad_func {
 	gpio_t gpio;
@@ -103,7 +101,7 @@ static void mtk_i2c_set_gpio_pinmux(uint8_t bus)
 	}
 }
 
-static void mtk_i2c_speed_init(uint8_t bus)
+static void mtk_i2c_speed_init_soc(uint8_t bus)
 {
 	uint8_t step_div;
 	const uint8_t clock_div = 5;
@@ -132,7 +130,7 @@ static void mtk_i2c_speed_init(uint8_t bus)
 
 void mtk_i2c_bus_init(uint8_t bus)
 {
-	mtk_i2c_speed_init(bus);
+	mtk_i2c_speed_init_soc(bus);
 	mtk_i2c_set_gpio_pinmux(bus);
 }
 
diff --git a/src/soc/mediatek/mt8183/include/soc/i2c.h b/src/soc/mediatek/mt8183/include/soc/i2c.h
index e70cf47958..ebe75facce 100644
--- a/src/soc/mediatek/mt8183/include/soc/i2c.h
+++ b/src/soc/mediatek/mt8183/include/soc/i2c.h
@@ -4,6 +4,7 @@
 #define SOC_MEDIATEK_MT8183_I2C_H
 
 #include <soc/i2c_common.h>
+#include <soc/pll.h>
 
 /* I2C Register */
 struct mt_i2c_regs {
@@ -39,6 +40,10 @@ struct mt_i2c_regs {
 	uint32_t rollback;
 };
 
+#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
+#define I2C_BUS_NUMBER 7
+#define MAX_CLOCK_DIV  32
+
 check_member(mt_i2c_regs, multi_dma, 0xf8c);
 
 void mtk_i2c_bus_init(uint8_t bus);
diff --git a/src/soc/mediatek/mt8192/i2c.c b/src/soc/mediatek/mt8192/i2c.c
index a76b0394c7..5f1e3a03f3 100644
--- a/src/soc/mediatek/mt8192/i2c.c
+++ b/src/soc/mediatek/mt8192/i2c.c
@@ -2,11 +2,9 @@
 
 #include <assert.h>
 #include <device/mmio.h>
-#include <soc/pll.h>
 #include <soc/i2c.h>
 #include <soc/gpio.h>
 
-#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
 #define I2C_FULL_DUTY 100
 #define I2C_HALF_DUTY 50
 #define I2C_ADJUSTED_DUTY 45
@@ -64,7 +62,8 @@ struct mtk_i2c mtk_i2c_bus_controller[] = {
 	},
 };
 
-#define I2C_BUS_NUMBER ARRAY_SIZE(mtk_i2c_bus_controller)
+_Static_assert(ARRAY_SIZE(mtk_i2c_bus_controller) == I2C_BUS_NUMBER,
+	       "Wrong size of mtk_i2c_bus_controller");
 
 struct pad_func {
 	gpio_t gpio;
@@ -127,7 +126,7 @@ static void mtk_i2c_set_gpio_pinmux(uint8_t bus)
 	}
 }
 
-static void mtk_i2c_speed_init(uint8_t bus)
+static void mtk_i2c_speed_init_soc(uint8_t bus)
 {
 	uint8_t step_div;
 	const uint8_t clock_div = 5;
@@ -178,7 +177,7 @@ static void mtk_i2c_speed_init(uint8_t bus)
 
 void mtk_i2c_bus_init(uint8_t bus)
 {
-	mtk_i2c_speed_init(bus);
+	mtk_i2c_speed_init_soc(bus);
 	mtk_i2c_set_gpio_pinmux(bus);
 }
 
diff --git a/src/soc/mediatek/mt8192/include/soc/i2c.h b/src/soc/mediatek/mt8192/include/soc/i2c.h
index 72a9af10e2..fbf41e6c48 100644
--- a/src/soc/mediatek/mt8192/include/soc/i2c.h
+++ b/src/soc/mediatek/mt8192/include/soc/i2c.h
@@ -4,6 +4,7 @@
 #define SOC_MEDIATEK_MT8192_I2C_H
 
 #include <soc/i2c_common.h>
+#include <soc/pll.h>
 
 /* I2C Register */
 struct mt_i2c_regs {
@@ -39,6 +40,10 @@ struct mt_i2c_regs {
 	uint32_t rollback;
 };
 
+#define I2C_BUS_NUMBER	10
+#define MAX_CLOCK_DIV	32
+#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
+
 check_member(mt_i2c_regs, multi_dma, 0xf8c);
 
 void mtk_i2c_bus_init(uint8_t bus);
diff --git a/src/soc/mediatek/mt8195/i2c.c b/src/soc/mediatek/mt8195/i2c.c
index 1a10eefe49..b401b79960 100644
--- a/src/soc/mediatek/mt8195/i2c.c
+++ b/src/soc/mediatek/mt8195/i2c.c
@@ -4,13 +4,10 @@
 #include <console/console.h>
 #include <device/mmio.h>
 #include <device/i2c_simple.h>
-#include <soc/pll.h>
 #include <soc/i2c.h>
 #include <soc/gpio.h>
 #include <timer.h>
 
-#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
-
 struct mtk_i2c mtk_i2c_bus_controller[] = {
 	[0] = {
 		.i2c_regs = (void *)(I2C_BASE),
@@ -54,7 +51,8 @@ struct mtk_i2c mtk_i2c_bus_controller[] = {
 	},
 };
 
-#define I2C_BUS_NUMBER ARRAY_SIZE(mtk_i2c_bus_controller)
+_Static_assert(ARRAY_SIZE(mtk_i2c_bus_controller) == I2C_BUS_NUMBER,
+	       "Wrong size of mtk_i2c_bus_controller");
 
 struct pad_func {
 	gpio_t gpio;
@@ -111,245 +109,6 @@ static void mtk_i2c_set_gpio_pinmux(uint8_t bus)
 	}
 }
 
-static int mtk_i2c_max_step_cnt(uint32_t target_speed)
-{
-	if (target_speed > I2C_SPEED_FAST_PLUS)
-		return MAX_HS_STEP_CNT_DIV;
-	else
-		return MAX_STEP_CNT_DIV;
-}
-
-/*
- * Check and calculate i2c ac-timing.
- *
- * Hardware design:
- * sample_ns = (1000000000 * (sample_cnt + 1)) / clk_src
- * xxx_cnt_div =  spec->min_xxx_ns / sample_ns
- *
- * The calculation of sample_ns is rounded down;
- * otherwise xxx_cnt_div would be greater than the smallest spec.
- * The sda_timing is chosen as the middle value between
- * the largest and smallest.
- */
-static int mtk_i2c_check_ac_timing(uint8_t bus, uint32_t clk_src,
-				   uint32_t check_speed,
-				   uint32_t step_cnt,
-				   uint32_t sample_cnt)
-{
-	const struct i2c_spec_values *spec;
-	uint32_t su_sta_cnt, low_cnt, high_cnt, max_step_cnt;
-	uint32_t sda_max, sda_min, clk_ns, max_sta_cnt = 0x100;
-	uint32_t sample_ns = ((uint64_t)NSECS_PER_SEC * (sample_cnt + 1)) / clk_src;
-	struct mtk_i2c_ac_timing *ac_timing;
-
-	spec = mtk_i2c_get_spec(check_speed);
-
-	clk_ns = NSECS_PER_SEC / clk_src;
-
-	su_sta_cnt = DIV_ROUND_UP(spec->min_su_sta_ns, clk_ns);
-	if (su_sta_cnt > max_sta_cnt)
-		return -1;
-
-	low_cnt = DIV_ROUND_UP(spec->min_low_ns, sample_ns);
-	max_step_cnt = mtk_i2c_max_step_cnt(check_speed);
-	if (2 * step_cnt > low_cnt && low_cnt < max_step_cnt) {
-		if (low_cnt > step_cnt) {
-			high_cnt = 2 * step_cnt - low_cnt;
-		} else {
-			high_cnt = step_cnt;
-			low_cnt = step_cnt;
-		}
-	} else {
-		return -2;
-	}
-
-	sda_max = spec->max_hd_dat_ns / sample_ns;
-	if (sda_max > low_cnt)
-		sda_max = 0;
-
-	sda_min = DIV_ROUND_UP(spec->min_su_dat_ns, sample_ns);
-	if (sda_min < low_cnt)
-		sda_min = 0;
-
-	if (sda_min > sda_max)
-		return -3;
-
-	ac_timing = &mtk_i2c_bus_controller[bus].ac_timing;
-	if (check_speed > I2C_SPEED_FAST_PLUS) {
-		ac_timing->hs = I2C_TIME_DEFAULT_VALUE | (sample_cnt << 12) | (high_cnt << 8);
-		ac_timing->ltiming &= ~GENMASK(15, 9);
-		ac_timing->ltiming |= (sample_cnt << 12) | (low_cnt << 9);
-		ac_timing->ext &= ~GENMASK(7, 1);
-		ac_timing->ext |= (su_sta_cnt << 1) | (1 << 0);
-	} else {
-		ac_timing->htiming = (sample_cnt << 8) | (high_cnt);
-		ac_timing->ltiming = (sample_cnt << 6) | (low_cnt);
-		ac_timing->ext = (su_sta_cnt << 8) | (1 << 0);
-	}
-
-	return 0;
-}
-
-/*
- * Calculate i2c port speed.
- *
- * Hardware design:
- * i2c_bus_freq = parent_clk / (clock_div * 2 * sample_cnt * step_cnt)
- * clock_div: fixed in hardware, but may be various in different SoCs
- *
- * To calculate sample_cnt and step_cnt, we pick the highest bus frequency
- * that is still no larger than i2c->speed_hz.
- */
-static int mtk_i2c_calculate_speed(uint8_t bus, uint32_t clk_src,
-				   uint32_t target_speed,
-				   uint32_t *timing_step_cnt,
-				   uint32_t *timing_sample_cnt)
-{
-	uint32_t step_cnt;
-	uint32_t sample_cnt;
-	uint32_t max_step_cnt;
-	uint32_t base_sample_cnt = MAX_SAMPLE_CNT_DIV;
-	uint32_t base_step_cnt;
-	uint32_t opt_div;
-	uint32_t best_mul;
-	uint32_t cnt_mul;
-	uint32_t clk_div = mtk_i2c_bus_controller[bus].ac_timing.inter_clk_div;
-	int32_t clock_div_constraint = 0;
-	int success = 0;
-
-	if (target_speed > I2C_SPEED_HIGH)
-		target_speed = I2C_SPEED_HIGH;
-
-	max_step_cnt = mtk_i2c_max_step_cnt(target_speed);
-	base_step_cnt = max_step_cnt;
-
-	/* Find the best combination */
-	opt_div = DIV_ROUND_UP(clk_src >> 1, target_speed);
-	best_mul = MAX_SAMPLE_CNT_DIV * max_step_cnt;
-
-	/* Search for the best pair (sample_cnt, step_cnt) with
-	 * 0 < sample_cnt < MAX_SAMPLE_CNT_DIV
-	 * 0 < step_cnt < max_step_cnt
-	 * sample_cnt * step_cnt >= opt_div
-	 * optimizing for sample_cnt * step_cnt being minimal
-	 */
-	for (sample_cnt = 1; sample_cnt <= MAX_SAMPLE_CNT_DIV; sample_cnt++) {
-		if (sample_cnt == 1) {
-			if (clk_div != 0)
-				clock_div_constraint = 1;
-			else
-				clock_div_constraint = 0;
-		} else {
-			if (clk_div > 1)
-				clock_div_constraint = 1;
-			else if (clk_div == 0)
-				clock_div_constraint = -1;
-			else
-				clock_div_constraint = 0;
-		}
-
-		step_cnt = DIV_ROUND_UP(opt_div + clock_div_constraint, sample_cnt);
-		if (step_cnt > max_step_cnt)
-			continue;
-
-		cnt_mul = step_cnt * sample_cnt;
-		if (cnt_mul >= best_mul)
-			continue;
-
-		if (mtk_i2c_check_ac_timing(bus, clk_src,
-					    target_speed, step_cnt - 1,
-					    sample_cnt - 1))
-			continue;
-
-		success = 1;
-		best_mul = cnt_mul;
-		base_sample_cnt = sample_cnt;
-		base_step_cnt = step_cnt;
-		if (best_mul == opt_div + clock_div_constraint)
-			break;
-
-	}
-
-	if (!success)
-		return -1;
-
-	sample_cnt = base_sample_cnt;
-	step_cnt = base_step_cnt;
-
-	if (clk_src / (2 * (sample_cnt * step_cnt - clock_div_constraint)) >
-	    target_speed)
-		return -1;
-
-	*timing_step_cnt = step_cnt - 1;
-	*timing_sample_cnt = sample_cnt - 1;
-
-	return 0;
-}
-
-static void mtk_i2c_speed_init(uint8_t bus, uint32_t speed)
-{
-	uint32_t max_clk_div = MAX_CLOCK_DIV;
-	uint32_t clk_src, clk_div, step_cnt, sample_cnt;
-	uint32_t l_step_cnt, l_sample_cnt;
-	uint32_t timing_reg_value, ltiming_reg_value;
-	struct mtk_i2c *bus_ctrl;
-
-	if (bus >= I2C_BUS_NUMBER) {
-		printk(BIOS_ERR, "%s, error bus num:%d\n", __func__, bus);
-		return;
-	}
-
-	bus_ctrl = &mtk_i2c_bus_controller[bus];
-
-	for (clk_div = 1; clk_div <= max_clk_div; clk_div++) {
-		clk_src = I2C_CLK_HZ / clk_div;
-		bus_ctrl->ac_timing.inter_clk_div = clk_div - 1;
-
-		if (speed > I2C_SPEED_FAST_PLUS) {
-			/* Set master code speed register */
-			if (mtk_i2c_calculate_speed(bus, clk_src, I2C_SPEED_FAST,
-						    &l_step_cnt, &l_sample_cnt))
-				continue;
-
-			timing_reg_value = (l_sample_cnt << 8) | l_step_cnt;
-
-			/* Set the high speed mode register */
-			if (mtk_i2c_calculate_speed(bus, clk_src, speed,
-						    &step_cnt, &sample_cnt))
-				continue;
-
-			ltiming_reg_value = (l_sample_cnt << 6) | l_step_cnt |
-					    (sample_cnt << 12) | (step_cnt << 9);
-			bus_ctrl->ac_timing.inter_clk_div = (clk_div - 1) << 8 | (clk_div - 1);
-		} else {
-			if (mtk_i2c_calculate_speed(bus, clk_src, speed,
-						    &l_step_cnt, &l_sample_cnt))
-				continue;
-
-			timing_reg_value = (l_sample_cnt << 8) | l_step_cnt;
-
-			/* Disable the high speed transaction */
-			bus_ctrl->ac_timing.hs = I2C_TIME_CLR_VALUE;
-
-			ltiming_reg_value = (l_sample_cnt << 6) | l_step_cnt;
-		}
-
-		break;
-	}
-
-	if (clk_div > max_clk_div) {
-		printk(BIOS_ERR, "%s, cannot support %d hz on i2c-%d\n", __func__, speed, bus);
-		return;
-	}
-
-	/* Init i2c bus timing register */
-	write32(&bus_ctrl->i2c_regs->clock_div, bus_ctrl->ac_timing.inter_clk_div);
-	write32(&bus_ctrl->i2c_regs->timing, bus_ctrl->ac_timing.htiming);
-	write32(&bus_ctrl->i2c_regs->ltiming, bus_ctrl->ac_timing.ltiming);
-	write32(&bus_ctrl->i2c_regs->hs, bus_ctrl->ac_timing.hs);
-	write32(&bus_ctrl->i2c_regs->ext_conf, bus_ctrl->ac_timing.ext);
-}
-
 void mtk_i2c_bus_init(uint8_t bus, uint32_t speed)
 {
 	mtk_i2c_speed_init(bus, speed);
@@ -362,3 +121,12 @@ void mtk_i2c_dump_more_info(struct mt_i2c_regs *regs)
 	       read32(&regs->ltiming),
 	       read32(&regs->clock_div));
 }
+
+void mtk_i2c_config_timing(struct mt_i2c_regs *regs, struct mtk_i2c *bus_ctrl)
+{
+	write32(&regs->clock_div, bus_ctrl->ac_timing.inter_clk_div);
+	write32(&regs->timing, bus_ctrl->ac_timing.htiming);
+	write32(&regs->ltiming, bus_ctrl->ac_timing.ltiming);
+	write32(&regs->hs, bus_ctrl->ac_timing.hs);
+	write32(&regs->ext_conf, bus_ctrl->ac_timing.ext);
+}
diff --git a/src/soc/mediatek/mt8195/include/soc/i2c.h b/src/soc/mediatek/mt8195/include/soc/i2c.h
index 743faa1e53..bbdc9e88b5 100644
--- a/src/soc/mediatek/mt8195/include/soc/i2c.h
+++ b/src/soc/mediatek/mt8195/include/soc/i2c.h
@@ -4,6 +4,7 @@
 #define SOC_MEDIATEK_MT8195_I2C_H
 
 #include <soc/i2c_common.h>
+#include <soc/pll.h>
 
 /* I2C Register */
 struct mt_i2c_regs {
@@ -52,7 +53,10 @@ enum {
 	I2C7,
 };
 
+#define I2C_BUS_NUMBER	8
 #define MAX_CLOCK_DIV	32
+#define I2C_CLK_HZ (UNIVPLL_HZ / 20)
+
 check_member(mt_i2c_regs, multi_dma, 0xf8c);
 
 void mtk_i2c_bus_init(uint8_t bus, uint32_t speed);