driver: esp32: I2C code refactoring

Use i2c_hal functions to enable support for
multiple SoCs.

Use DT compat to enable I2C from device
tree configuration

Signed-off-by: Sylvio Alves <sylvio.alves@espressif.com>
diff --git a/boards/riscv/esp32c3_devkitm/esp32c3_devkitm.dts b/boards/riscv/esp32c3_devkitm/esp32c3_devkitm.dts
index 995659a..ece4e0b 100644
--- a/boards/riscv/esp32c3_devkitm/esp32c3_devkitm.dts
+++ b/boards/riscv/esp32c3_devkitm/esp32c3_devkitm.dts
@@ -21,6 +21,7 @@
 
 	aliases {
 		sw0 = &user_button1;
+		i2c-0 = &i2c0;
 	};
 
 	gpio_keys {
@@ -43,6 +44,13 @@
 	rx-pin = <3>;
 };
 
+&i2c0 {
+	status = "okay";
+	clock-frequency = <I2C_BITRATE_STANDARD>;
+	sda-pin = <2>;
+	scl-pin = <3>;
+};
+
 &trng0 {
 	status = "okay";
 };
diff --git a/boards/riscv/esp32c3_devkitm/esp32c3_devkitm_defconfig b/boards/riscv/esp32c3_devkitm/esp32c3_devkitm_defconfig
index 3bc152f..057993d 100644
--- a/boards/riscv/esp32c3_devkitm/esp32c3_devkitm_defconfig
+++ b/boards/riscv/esp32c3_devkitm/esp32c3_devkitm_defconfig
@@ -13,4 +13,5 @@
 CONFIG_PINMUX_ESP32=y
 CONFIG_GPIO=y
 CONFIG_GPIO_ESP32=y
+CONFIG_I2C=y
 CONFIG_CLOCK_CONTROL=y
diff --git a/boards/xtensa/esp32/esp32.dts b/boards/xtensa/esp32/esp32.dts
index 9267ace..e06b0ff 100644
--- a/boards/xtensa/esp32/esp32.dts
+++ b/boards/xtensa/esp32/esp32.dts
@@ -69,13 +69,13 @@
 
 &i2c0 {
 	status = "okay";
-	clock-frequency = <I2C_BITRATE_FAST>;
+	clock-frequency = <I2C_BITRATE_STANDARD>;
 	sda-pin = <21>;
 	scl-pin = <22>;
 };
 
 &i2c1 {
-	clock-frequency = <I2C_BITRATE_FAST>;
+	clock-frequency = <I2C_BITRATE_STANDARD>;
 	sda-pin = <18>;
 	scl-pin = <5>;
 };
diff --git a/boards/xtensa/esp32/esp32_defconfig b/boards/xtensa/esp32/esp32_defconfig
index caed317..01596c3 100644
--- a/boards/xtensa/esp32/esp32_defconfig
+++ b/boards/xtensa/esp32/esp32_defconfig
@@ -25,9 +25,6 @@
 CONFIG_GEN_IRQ_VECTOR_TABLE=n
 
 CONFIG_I2C=y
-CONFIG_I2C_ESP32=y
-CONFIG_I2C_0=y
-CONFIG_I2C_1=y
 CONFIG_CLOCK_CONTROL=y
 
 CONFIG_BOOTLOADER_ESP_IDF=y
diff --git a/boards/xtensa/esp32s2_saola/esp32s2_saola.dts b/boards/xtensa/esp32s2_saola/esp32s2_saola.dts
index 15168d1..f290ebf 100644
--- a/boards/xtensa/esp32s2_saola/esp32s2_saola.dts
+++ b/boards/xtensa/esp32s2_saola/esp32s2_saola.dts
@@ -12,6 +12,10 @@
 	model = "esp32s2_saola";
 	compatible = "espressif,esp32s2";
 
+	aliases {
+		i2c-0 = &i2c0;
+	};
+
 	chosen {
 		zephyr,sram = &sram0;
 		zephyr,console = &uart0;
@@ -55,6 +59,19 @@
 	status = "okay";
 };
 
+&i2c0 {
+	status = "okay";
+	clock-frequency = <I2C_BITRATE_STANDARD>;
+	sda-pin = <8>;
+	scl-pin = <9>;
+};
+
+&i2c1 {
+	clock-frequency = <I2C_BITRATE_STANDARD>;
+	sda-pin = <3>;
+	scl-pin = <4>;
+};
+
 &trng0 {
 	status = "okay";
 };
diff --git a/boards/xtensa/esp32s2_saola/esp32s2_saola_defconfig b/boards/xtensa/esp32s2_saola/esp32s2_saola_defconfig
index 0842d60..68bd05b 100644
--- a/boards/xtensa/esp32s2_saola/esp32s2_saola_defconfig
+++ b/boards/xtensa/esp32s2_saola/esp32s2_saola_defconfig
@@ -23,6 +23,7 @@
 CONFIG_GEN_ISR_TABLES=y
 CONFIG_GEN_IRQ_VECTOR_TABLE=n
 
+CONFIG_I2C=y
 CONFIG_CLOCK_CONTROL=y
 
 CONFIG_BOOTLOADER_ESP_IDF=y
diff --git a/boards/xtensa/odroid_go/odroid_go_defconfig b/boards/xtensa/odroid_go/odroid_go_defconfig
index 242dd07..025325a 100644
--- a/boards/xtensa/odroid_go/odroid_go_defconfig
+++ b/boards/xtensa/odroid_go/odroid_go_defconfig
@@ -26,5 +26,3 @@
 CONFIG_GEN_IRQ_VECTOR_TABLE=n
 
 CONFIG_I2C=y
-CONFIG_I2C_ESP32=y
-CONFIG_I2C_0=y
diff --git a/drivers/i2c/Kconfig.esp32 b/drivers/i2c/Kconfig.esp32
index f2fa34e..bfc1674 100644
--- a/drivers/i2c/Kconfig.esp32
+++ b/drivers/i2c/Kconfig.esp32
@@ -1,45 +1,15 @@
 # ESP32 I2C configuration options
 
 # Copyright (c) 2017 Intel Corporation
+# Copyright (c) 2021 Espressif Systems (Shanghai) Co., Ltd.
 # SPDX-License-Identifier: Apache-2.0
 
-menuconfig I2C_ESP32
-	bool "ESP32 I2C"
-	depends on SOC_ESP32
+DT_COMPAT_ESP32_I2C := espressif,esp32-i2c
+
+config I2C_ESP32
+	bool "ESP32 I2C driver"
+	depends on SOC_ESP32 || SOC_ESP32S2 || SOC_ESP32C3
+	default $(dt_compat_enabled,$(DT_COMPAT_ESP32_I2C))
 	select GPIO_ESP32
 	help
 	  Enables the ESP32 I2C driver
-
-if I2C_ESP32
-
-config I2C_0
-	bool "Enable I2C Port 0"
-
-config I2C_1
-	bool "Enable I2C Port 1"
-
-config I2C_ESP32_TIMEOUT
-	int "I2C timeout to receive a data bit in APB clock cycles"
-	default 200000
-
-if I2C_0
-
-config I2C_ESP32_0_TX_LSB_FIRST
-	bool "Port 0 Transmit LSB first"
-
-config I2C_ESP32_0_RX_LSB_FIRST
-	bool "Port 0 Receive LSB first"
-
-endif # I2C_0
-
-if I2C_1
-
-config I2C_ESP32_1_TX_LSB_FIRST
-	bool "Port 1 Transmit LSB first"
-
-config I2C_ESP32_1_RX_LSB_FIRST
-	bool "Port 1 Receive LSB first"
-
-endif # I2C_1
-
-endif # I2C_ESP32
diff --git a/drivers/i2c/i2c_esp32.c b/drivers/i2c/i2c_esp32.c
index 540e29e..6a093dd 100644
--- a/drivers/i2c/i2c_esp32.c
+++ b/drivers/i2c/i2c_esp32.c
@@ -8,62 +8,65 @@
 #define DT_DRV_COMPAT espressif_esp32_i2c
 
 /* Include esp-idf headers first to avoid redefining BIT() macro */
-#include <soc/dport_reg.h>
 #include <soc/i2c_reg.h>
 #include <esp32/rom/gpio.h>
 #include <soc/gpio_sig_map.h>
+#include <hal/i2c_ll.h>
+#include <hal/i2c_hal.h>
+#include <hal/gpio_hal.h>
 
 #include <soc.h>
 #include <errno.h>
 #include <drivers/gpio.h>
-#include <drivers/gpio/gpio_esp32.h>
 #include <drivers/i2c.h>
 #include <drivers/interrupt_controller/intc_esp32.h>
 #include <drivers/clock_control.h>
 #include <sys/util.h>
 #include <string.h>
 
-#define LOG_LEVEL CONFIG_I2C_LOG_LEVEL
 #include <logging/log.h>
-LOG_MODULE_REGISTER(i2c_esp32);
+LOG_MODULE_REGISTER(i2c_esp32, CONFIG_I2C_LOG_LEVEL);
 
 #include "i2c-priv.h"
 
-/* Number of entries in hardware command queue */
-#define I2C_ESP32_NUM_CMDS 16
-/* Number of bytes in hardware FIFO */
-#define I2C_ESP32_BUFFER_SIZE 32
+#define I2C_FILTER_CYC_NUM_DEF 7	/* Number of apb cycles filtered by default */
+#define I2C_CLR_BUS_SCL_NUM 9	/* Number of SCL clocks to restore SDA signal */
+#define I2C_CLR_BUS_HALF_PERIOD_US 5	/* Period of SCL clock to restore SDA signal */
+#define I2C_TRANSFER_TIMEOUT_MSEC 500	/* Transfer timeout period */
 
-#define I2C_ESP32_TRANSFER_TIMEOUT_MS 100
-#define I2C_ESP32_TIMEOUT_USEC  1000
+/* Freq limitation when using different clock sources */
+#define I2C_CLK_LIMIT_REF_TICK (1 * 1000 * 1000 / 20)	/* REF_TICK, no more than REF_TICK/20*/
+#define I2C_CLK_LIMIT_APB (80 * 1000 * 1000 / 20)	/* Limited by APB, no more than APB/20 */
+#define I2C_CLK_LIMIT_RTC (20 * 1000 * 1000 / 20)	/* Limited by RTC, no more than RTC/20 */
+#define I2C_CLK_LIMIT_XTAL (40 * 1000 * 1000 / 20)	/* Limited by RTC, no more than XTAL/20 */
 
-enum i2c_esp32_opcodes {
-	I2C_ESP32_OP_RSTART,
-	I2C_ESP32_OP_WRITE,
-	I2C_ESP32_OP_READ,
-	I2C_ESP32_OP_STOP,
-	I2C_ESP32_OP_END
+enum i2c_status_t {
+	I2C_STATUS_READ,	/* read status for current master command */
+	I2C_STATUS_WRITE,	/* write status for current master command */
+	I2C_STATUS_IDLE,	/* idle status for current master command */
+	I2C_STATUS_ACK_ERROR,	/* ack error status for current master command */
+	I2C_STATUS_DONE,	/* I2C command done */
+	I2C_STATUS_TIMEOUT,	/* I2C bus status error, and operation timeout */
 };
 
-struct i2c_esp32_cmd {
-	uint32_t num_bytes : 8;
-	uint32_t ack_en : 1;
-	uint32_t ack_exp : 1;
-	uint32_t ack_val : 1;
-	uint32_t opcode : 3;
-	uint32_t reserved : 17;
-	uint32_t done : 1;
+struct i2c_esp32_pin {
+	const char *gpio_name;
+	int sig_out;
+	int sig_in;
+	gpio_pin_t pin;
 };
 
 struct i2c_esp32_data {
-	uint32_t dev_config;
-	uint16_t address;
-	uint32_t err_status;
-
-	struct k_sem fifo_sem;
+	i2c_hal_context_t hal;
+	struct k_sem cmd_sem;
 	struct k_sem transfer_sem;
-
+	volatile enum i2c_status_t status;
+	uint32_t dev_config;
+	int cmd_idx;
 	int irq_line;
+
+	const struct device *scl_gpio;
+	const struct device *sda_gpio;
 };
 
 typedef void (*irq_connect_cb)(void);
@@ -73,19 +76,10 @@
 
 	const struct device *clock_dev;
 
-	const struct {
-		int sda_out;
-		int sda_in;
-		int scl_out;
-		int scl_in;
-	} sig;
+	const struct i2c_esp32_pin scl;
+	const struct i2c_esp32_pin sda;
 
-	const struct {
-		int scl;
-		int sda;
-	} pins;
-
-	const clock_control_subsys_t peripheral_id;
+	const clock_control_subsys_t clock_subsys;
 
 	const struct {
 		bool tx_lsb_first;
@@ -98,83 +92,149 @@
 	const uint32_t bitrate;
 };
 
-static int i2c_esp32_configure_pins(int pin, int matrix_out, int matrix_in)
+/* I2C clock characteristic, The order is the same as i2c_sclk_t. */
+static uint32_t i2c_clk_alloc[I2C_SCLK_MAX] = {
+	0,
+#if SOC_I2C_SUPPORT_APB
+	I2C_CLK_LIMIT_APB,	/* I2C APB clock characteristic */
+#endif
+#if SOC_I2C_SUPPORT_XTAL
+	I2C_CLK_LIMIT_XTAL,	/* I2C XTAL characteristic */
+#endif
+#if SOC_I2C_SUPPORT_RTC
+	I2C_CLK_LIMIT_RTC,	/* I2C 20M RTC characteristic */
+#endif
+#if SOC_I2C_SUPPORT_REF_TICK
+	I2C_CLK_LIMIT_REF_TICK,	/* I2C REF_TICK characteristic */
+#endif
+};
+
+static i2c_sclk_t i2c_get_clk_src(uint32_t clk_freq)
 {
-	const int pin_mode = GPIO_OUTPUT_HIGH |
-			     GPIO_OPEN_DRAIN  |
-			     GPIO_PULL_UP;
-	const char *device_name = gpio_esp32_get_gpio_for_pin(pin);
-	const struct device *gpio;
-	int ret;
-
-	if (!device_name) {
-		return -EINVAL;
+	for (i2c_sclk_t clk = I2C_SCLK_DEFAULT + 1; clk < I2C_SCLK_MAX; clk++) {
+		if (clk_freq <= i2c_clk_alloc[clk]) {
+			return clk;
+		}
 	}
-	gpio = device_get_binding(device_name);
-	if (!gpio) {
+	return I2C_SCLK_MAX;	/* flag invalid */
+}
+
+static int i2c_esp32_config_pin(const struct device *dev)
+{
+	const struct i2c_esp32_config *config = dev->config;
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+
+	if (config->index >= SOC_I2C_NUM) {
+		LOG_ERR("Invalid I2C peripheral number");
 		return -EINVAL;
 	}
 
-	ret = gpio_pin_configure(gpio, pin, pin_mode);
-	if (ret < 0) {
-		return ret;
-	}
+	gpio_pin_set(data->sda_gpio, config->sda.pin, 1);
+	gpio_pin_configure(data->sda_gpio, config->sda.pin,
+			GPIO_PULL_UP | GPIO_OPEN_DRAIN | GPIO_OUTPUT | GPIO_INPUT);
+	esp_rom_gpio_matrix_out(config->sda.pin, config->sda.sig_out, 0, 0);
+	esp_rom_gpio_matrix_in(config->sda.pin, config->sda.sig_in, 0);
 
-	esp_rom_gpio_matrix_out(pin, matrix_out, false, false);
-	esp_rom_gpio_matrix_in(pin, matrix_in, false);
+	gpio_pin_set(data->scl_gpio, config->scl.pin, 1);
+	gpio_pin_configure(data->scl_gpio, config->scl.pin,
+			GPIO_PULL_UP | GPIO_OPEN_DRAIN | GPIO_OUTPUT | GPIO_INPUT);
+	esp_rom_gpio_matrix_out(config->scl.pin, config->scl.sig_out, 0, 0);
+	esp_rom_gpio_matrix_in(config->scl.pin, config->scl.sig_in, 0);
 
 	return 0;
 }
 
-static int i2c_esp32_configure_speed(const struct device *dev,
-				     uint32_t speed)
+/* Some slave device will die by accident and keep the SDA in low level,
+ * in this case, master should send several clock to make the slave release the bus.
+ * Slave mode of ESP32 might also get in wrong state that held the SDA low,
+ * in this case, master device could send a stop signal to make esp32 slave release the bus.
+ **/
+static void IRAM_ATTR i2c_master_clear_bus(const struct device *dev)
 {
-	static const uint32_t speed_to_freq_tbl[] = {
-		[I2C_SPEED_STANDARD] = KHZ(100),
-		[I2C_SPEED_FAST] = KHZ(400),
-		[I2C_SPEED_FAST_PLUS] = MHZ(1),
-		[I2C_SPEED_HIGH] = 0,
-		[I2C_SPEED_ULTRA] = 0
-	};
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
 
+#ifndef SOC_I2C_SUPPORT_HW_CLR_BUS
 	const struct i2c_esp32_config *config = dev->config;
+	const int scl_half_period = I2C_CLR_BUS_HALF_PERIOD_US; /* use standard 100kHz data rate */
+	int i = 0;
+	gpio_pin_t scl_io = config->scl.pin;
+	gpio_pin_t sda_io = config->sda.pin;
 
-	uint32_t sys_clk_freq = 0;
-	uint32_t freq_hz = speed_to_freq_tbl[speed];
-	uint32_t period;
-
-	if (!freq_hz) {
-		return -ENOTSUP;
+	gpio_pin_configure(data->scl_gpio, scl_io, GPIO_OUTPUT | GPIO_OPEN_DRAIN);
+	gpio_pin_configure(data->sda_gpio, sda_io, GPIO_OUTPUT | GPIO_OPEN_DRAIN | GPIO_INPUT);
+	/* If a SLAVE device was in a read operation when the bus was interrupted, */
+	/* the SLAVE device is controlling SDA. If the slave is sending a stream of ZERO bytes, */
+	/* it will only release SDA during the  ACK bit period. So, this reset code needs */
+	/* to synchronize the bit stream with either the ACK bit, or a 1 bit to correctly */
+	/* generate a STOP condition. */
+	gpio_pin_set(data->scl_gpio, scl_io, 0);
+	gpio_pin_set(data->sda_gpio, sda_io, 1);
+	esp_rom_delay_us(scl_half_period);
+	while (!gpio_pin_get(data->sda_gpio, sda_io) && (i++ < I2C_CLR_BUS_SCL_NUM)) {
+		gpio_pin_set(data->scl_gpio, scl_io, 1);
+		esp_rom_delay_us(scl_half_period);
+		gpio_pin_set(data->scl_gpio, scl_io, 0);
+		esp_rom_delay_us(scl_half_period);
 	}
+	gpio_pin_set(data->sda_gpio, sda_io, 0); /* setup for STOP */
+	gpio_pin_set(data->scl_gpio, scl_io, 1);
+	esp_rom_delay_us(scl_half_period);
+	gpio_pin_set(data->sda_gpio, sda_io, 1); /* STOP, SDA low -> high while SCL is HIGH */
+	i2c_esp32_config_pin(dev);
+#else
+	i2c_hal_master_clr_bus(&data->hal);
+#endif
+	i2c_hal_update_config(&data->hal);
+}
 
-	if (clock_control_get_rate(config->clock_dev,
-				   config->peripheral_id,
-				   &sys_clk_freq)) {
-		return -EINVAL;
-	}
+static void IRAM_ATTR i2c_hw_fsm_reset(const struct device *dev)
+{
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
 
-	period = (sys_clk_freq / freq_hz);
-	period /= 2U; /* Set hold and setup times to 1/2th of period */
+#ifndef SOC_I2C_SUPPORT_HW_FSM_RST
+	const struct i2c_esp32_config *config = dev->config;
+	int scl_low_period, scl_high_period;
+	int scl_start_hold, scl_rstart_setup;
+	int scl_stop_hold, scl_stop_setup;
+	int sda_hold, sda_sample;
+	int timeout;
+	uint8_t filter_cfg;
 
-	esp32_set_mask32(period << I2C_SCL_LOW_PERIOD_S,
-		   I2C_SCL_LOW_PERIOD_REG(config->index));
-	esp32_set_mask32(period << I2C_SCL_HIGH_PERIOD_S,
-		   I2C_SCL_HIGH_PERIOD_REG(config->index));
+	i2c_hal_get_scl_timing(&data->hal, &scl_high_period, &scl_low_period);
+	i2c_hal_get_start_timing(&data->hal, &scl_rstart_setup, &scl_start_hold);
+	i2c_hal_get_stop_timing(&data->hal, &scl_stop_setup, &scl_stop_hold);
+	i2c_hal_get_sda_timing(&data->hal, &sda_sample, &sda_hold);
+	i2c_hal_get_tout(&data->hal, &timeout);
+	i2c_hal_get_filter(&data->hal, &filter_cfg);
 
-	esp32_set_mask32(period << I2C_SCL_START_HOLD_TIME_S,
-		   I2C_SCL_START_HOLD_REG(config->index));
-	esp32_set_mask32(period << I2C_SCL_RSTART_SETUP_TIME_S,
-		   I2C_SCL_RSTART_SETUP_REG(config->index));
-	esp32_set_mask32(period << I2C_SCL_STOP_HOLD_TIME_S,
-		   I2C_SCL_STOP_HOLD_REG(config->index));
-	esp32_set_mask32(period << I2C_SCL_STOP_SETUP_TIME_S,
-		   I2C_SCL_STOP_SETUP_REG(config->index));
+	/* to reset the I2C hw module, we need re-enable the hw */
+	clock_control_off(config->clock_dev, config->clock_subsys);
+	i2c_master_clear_bus(dev);
+	clock_control_on(config->clock_dev, config->clock_subsys);
 
-	period /= 2U; /* Set sample and hold times to 1/4th of period */
-	esp32_set_mask32(period << I2C_SDA_HOLD_TIME_S,
-		   I2C_SDA_HOLD_REG(config->index));
-	esp32_set_mask32(period << I2C_SDA_SAMPLE_TIME_S,
-		   I2C_SDA_SAMPLE_REG(config->index));
+	i2c_hal_master_init(&data->hal, config->index);
+	i2c_hal_disable_intr_mask(&data->hal, I2C_LL_INTR_MASK);
+	i2c_hal_clr_intsts_mask(&data->hal, I2C_LL_INTR_MASK);
+	i2c_hal_set_scl_timing(&data->hal, scl_high_period, scl_low_period);
+	i2c_hal_set_start_timing(&data->hal, scl_rstart_setup, scl_start_hold);
+	i2c_hal_set_stop_timing(&data->hal, scl_stop_setup, scl_stop_hold);
+	i2c_hal_set_sda_timing(&data->hal, sda_sample, sda_hold);
+	i2c_hal_set_tout(&data->hal, timeout);
+	i2c_hal_set_filter(&data->hal, filter_cfg);
+#else
+	i2c_hal_master_fsm_rst(&data->hal);
+	i2c_master_clear_bus(dev);
+#endif
+	i2c_hal_update_config(&data->hal);
+}
+
+static int i2c_esp32_recover(const struct device *dev)
+{
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+
+	k_sem_take(&data->transfer_sem, K_FOREVER);
+	i2c_hw_fsm_reset(dev);
+	k_sem_give(&data->transfer_sem);
 
 	return 0;
 }
@@ -182,340 +242,300 @@
 static int i2c_esp32_configure(const struct device *dev, uint32_t dev_config)
 {
 	const struct i2c_esp32_config *config = dev->config;
-	struct i2c_esp32_data *data = dev->data;
-	unsigned int key = irq_lock();
-	uint32_t v = 0U;
-	int ret;
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
 
-	ret = i2c_esp32_configure_pins(config->pins.scl,
-				       config->sig.scl_out,
-				       config->sig.scl_in);
-	if (ret < 0) {
-		return ret;
+	if (!(dev_config & I2C_MODE_MASTER)) {
+		LOG_ERR("Only I2C Master mode supported.");
+		return -ENOTSUP;
 	}
 
-	ret = i2c_esp32_configure_pins(config->pins.sda,
-				       config->sig.sda_out,
-				       config->sig.sda_in);
-	if (ret < 0) {
-		return ret;
-	}
+	data->dev_config = dev_config;
 
-	clock_control_on(config->clock_dev, config->peripheral_id);
+	i2c_trans_mode_t tx_mode = I2C_DATA_MODE_MSB_FIRST;
+	i2c_trans_mode_t rx_mode = I2C_DATA_MODE_MSB_FIRST;
 
-	/* MSB or LSB first is configurable for both TX and RX */
 	if (config->mode.tx_lsb_first) {
-		v |= I2C_TX_LSB_FIRST;
+		tx_mode = I2C_DATA_MODE_LSB_FIRST;
 	}
 
 	if (config->mode.rx_lsb_first) {
-		v |= I2C_RX_LSB_FIRST;
+		rx_mode = I2C_DATA_MODE_LSB_FIRST;
 	}
 
-	if (dev_config & I2C_MODE_MASTER) {
-		v |= I2C_MS_MODE;
-		sys_write32(0, I2C_SLAVE_ADDR_REG(config->index));
-	} else {
-		uint32_t addr = (data->address & I2C_SLAVE_ADDR_V);
+	i2c_hal_master_init(&data->hal, config->index);
+	i2c_hal_set_data_mode(&data->hal, tx_mode, rx_mode);
+	i2c_hal_set_filter(&data->hal, I2C_FILTER_CYC_NUM_DEF);
+	i2c_hal_update_config(&data->hal);
 
-		if (dev_config & I2C_ADDR_10_BITS) {
-			addr |= I2C_ADDR_10BIT_EN;
-		}
-		sys_write32(addr << I2C_SLAVE_ADDR_S,
-			    I2C_SLAVE_ADDR_REG(config->index));
-
-		/* Before setting up FIFO and interrupts, stop transmission */
-		sys_clear_bit(I2C_CTR_REG(config->index), I2C_TRANS_START_S);
-
-		/* Byte after address isn't the offset address in slave RAM */
-		sys_clear_bit(I2C_FIFO_CONF_REG(config->index),
-			      I2C_FIFO_ADDR_CFG_EN_S);
+	if (config->bitrate == 0) {
+		LOG_ERR("Error configuring I2C speed.");
+		return -ENOTSUP;
 	}
 
-	/* Use open-drain for clock and data pins */
-	v |= (I2C_SCL_FORCE_OUT | I2C_SDA_FORCE_OUT);
-	v |= I2C_CLK_EN;
-	sys_write32(v, I2C_CTR_REG(config->index));
-
-	ret = i2c_esp32_configure_speed(dev, I2C_SPEED_GET(dev_config));
-	if (ret < 0) {
-		goto out;
-	}
-
-	/* Use FIFO to transmit data */
-	sys_clear_bit(I2C_FIFO_CONF_REG(config->index), I2C_NONFIFO_EN_S);
-
-	v = CONFIG_I2C_ESP32_TIMEOUT & I2C_TIME_OUT_REG;
-	sys_write32(v << I2C_TIME_OUT_REG_S, I2C_TO_REG(config->index));
-
-	/* Enable interrupt types handled by the ISR */
-	sys_write32(I2C_ACK_ERR_INT_ENA_M |
-		    I2C_TIME_OUT_INT_ENA_M |
-		    I2C_TRANS_COMPLETE_INT_ENA_M |
-		    I2C_ARBITRATION_LOST_INT_ENA_M,
-		    I2C_INT_ENA_REG(config->index));
+	i2c_hal_set_bus_timing(&data->hal, config->bitrate, i2c_get_clk_src(config->bitrate));
+	i2c_hal_update_config(&data->hal);
 
 	irq_enable(data->irq_line);
 
-out:
-	irq_unlock(key);
+	return 0;
+}
+
+static void IRAM_ATTR i2c_esp32_reset_fifo(const struct device *dev)
+{
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+
+	/* reset fifo buffers */
+	i2c_hal_txfifo_rst(&data->hal);
+	i2c_hal_rxfifo_rst(&data->hal);
+}
+
+static int IRAM_ATTR i2c_esp32_transmit(const struct device *dev)
+{
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+
+	/* start the transfer */
+	i2c_hal_update_config(&data->hal);
+	i2c_hal_trans_start(&data->hal);
+
+	int ret = k_sem_take(&data->cmd_sem, K_MSEC(I2C_TRANSFER_TIMEOUT_MSEC));
+
+	if (ret != 0) {
+		/* If the I2C slave is powered off or the SDA/SCL is */
+		/* connected to ground, for example, I2C hw FSM would get */
+		/* stuck in wrong state, we have to reset the I2C module in this case. */
+		i2c_hw_fsm_reset(dev);
+		return -ETIMEDOUT;
+	}
+
+	if (data->status == I2C_STATUS_TIMEOUT) {
+		i2c_hw_fsm_reset(dev);
+		ret = -ETIMEDOUT;
+	} else if (data->status == I2C_STATUS_ACK_ERROR) {
+		ret = -EFAULT;
+	} else {
+		ret = 0;
+	}
 
 	return ret;
 }
 
-static inline void i2c_esp32_reset_fifo(const struct i2c_esp32_config *config)
+static void IRAM_ATTR i2c_esp32_write_addr(const struct device *dev, uint16_t addr)
 {
-	uint32_t reg = I2C_FIFO_CONF_REG(config->index);
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+	uint8_t addr_len = 1;
+	uint8_t addr_byte = addr & 0xFF;
 
-	/* Writing 1 and then 0 to these bits will reset the I2C fifo */
-	esp32_set_mask32(I2C_TX_FIFO_RST | I2C_RX_FIFO_RST, reg);
-	esp32_clear_mask32(I2C_TX_FIFO_RST | I2C_RX_FIFO_RST, reg);
-}
+	i2c_hw_cmd_t cmd = {
+		.op_code = I2C_LL_CMD_RESTART
+	};
 
-static int i2c_esp32_wait_timeout(uint32_t *counter)
-{
-	if (*counter == 0) {
-		return 1;
-	}
+	/* write re-start command */
+	i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 
-	(*counter)--;
-	k_busy_wait(1);
-	return 0;
-}
-
-static int i2c_esp32_wait(const struct device *dev,
-			  volatile struct i2c_esp32_cmd *wait_cmd)
-{
-	const struct i2c_esp32_config *config = dev->config;
-	struct i2c_esp32_data *data = dev->data;
-	uint32_t timeout = I2C_ESP32_TIMEOUT_USEC;
-
-	if (wait_cmd) {
-		while (!wait_cmd->done) {
-			if (i2c_esp32_wait_timeout(&timeout)) {
-				return -ETIMEDOUT;
-			}
-		}
-	}
-
-	/* Wait for I2C bus to finish its business */
-	timeout = I2C_ESP32_TIMEOUT_USEC;
-	while (sys_read32(I2C_SR_REG(config->index)) & I2C_BUS_BUSY) {
-		if (i2c_esp32_wait_timeout(&timeout)) {
-			return -ETIMEDOUT;
-		}
-	}
-
-	uint32_t status = data->err_status;
-	if (status & (I2C_ARBITRATION_LOST_INT_RAW | I2C_ACK_ERR_INT_RAW)) {
-		data->err_status = 0;
-		return -EIO;
-	}
-	if (status & I2C_TIME_OUT_INT_RAW) {
-		data->err_status = 0;
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-}
-
-static int i2c_esp32_transmit(const struct device *dev, volatile struct i2c_esp32_cmd *wait_cmd)
-{
-	const struct i2c_esp32_config *config = dev->config;
-	struct i2c_esp32_data *data = dev->data;
-
-	/* Start transmission and wait for the ISR to give the semaphore */
-	sys_set_bit(I2C_CTR_REG(config->index), I2C_TRANS_START_S);
-	if (k_sem_take(&data->fifo_sem, K_MSEC(I2C_ESP32_TRANSFER_TIMEOUT_MS)) < 0) {
-		return -ETIMEDOUT;
-	}
-
-	return i2c_esp32_wait(dev, wait_cmd);
-}
-
-static volatile struct i2c_esp32_cmd *
-i2c_esp32_write_addr(const struct device *dev,
-		     volatile struct i2c_esp32_cmd *cmd,
-		     struct i2c_msg *msg,
-		     uint16_t addr)
-{
-	const struct i2c_esp32_config *config = dev->config;
-	struct i2c_esp32_data *data = dev->data;
-	uint32_t addr_len = 1U;
-
-	i2c_esp32_reset_fifo(config);
-
-	sys_write32(addr & I2C_FIFO_RDATA, I2C_DATA_APB_REG(config->index));
+	/* write address value in tx buffer */
+	i2c_hal_write_txfifo(&data->hal, &addr_byte, 1);
 	if (data->dev_config & I2C_ADDR_10_BITS) {
-		sys_write32(I2C_DATA_APB_REG(config->index),
-			    (addr >> 8) & I2C_FIFO_RDATA);
+		addr_byte = (addr >> 8) & 0xFF;
+		i2c_hal_write_txfifo(&data->hal, &addr_byte, 1);
 		addr_len++;
 	}
 
-	if ((msg->flags & I2C_MSG_RW_MASK) != I2C_MSG_WRITE) {
-		*cmd++ = (struct i2c_esp32_cmd) {
-			.opcode = I2C_ESP32_OP_WRITE,
-			.ack_en = true,
-			.num_bytes = addr_len,
-		};
-	} else {
-		msg->len += addr_len;
-	}
+	cmd = (i2c_hw_cmd_t) {
+		.op_code = I2C_LL_CMD_WRITE,
+		.ack_en = true,
+		.byte_num = addr_len,
+	};
 
-	return cmd;
+	i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 }
 
-static int i2c_esp32_read_msg(const struct device *dev, uint16_t addr,
-			      struct i2c_msg msg)
+static int IRAM_ATTR i2c_esp32_read_msg(const struct device *dev,
+	struct i2c_msg *msg, uint16_t addr)
 {
-	const struct i2c_esp32_config *config = dev->config;
-	volatile struct i2c_esp32_cmd *cmd =
-		(void *)I2C_COMD0_REG(config->index);
-	uint32_t i;
-	int ret;
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+	uint8_t rd_filled = 0;
+	uint8_t *read_pr = NULL;
+	int ret = 0;
+
+	/* reset command index and set status as read operation */
+	data->cmd_idx = 0;
+	data->status = I2C_STATUS_READ;
+
+	i2c_hw_cmd_t cmd = {
+		.op_code = I2C_LL_CMD_READ
+	};
+
+	i2c_hw_cmd_t hw_end_cmd = {
+		.op_code = I2C_LL_CMD_END,
+	};
 
 	/* Set the R/W bit to R */
 	addr |= BIT(0);
 
-	*cmd++ = (struct i2c_esp32_cmd) {
-		.opcode = I2C_ESP32_OP_RSTART
-	};
+	/* write restart command and address */
+	i2c_esp32_write_addr(dev, addr);
 
-	cmd = i2c_esp32_write_addr(dev, cmd, &msg, addr);
+	while (msg->len) {
+		rd_filled = (msg->len > SOC_I2C_FIFO_LEN) ? SOC_I2C_FIFO_LEN : (msg->len - 1);
+		read_pr = msg->buf;
+		msg->len -= rd_filled;
 
-	for (; msg.len; cmd = (void *)I2C_COMD0_REG(config->index)) {
-		volatile struct i2c_esp32_cmd *wait_cmd = NULL;
-		uint32_t to_read = MIN(I2C_ESP32_BUFFER_SIZE, msg.len - 1);
-
-		/* Might be the last byte, in which case, `to_read` will
-		 * be 0 here.  See comment below.
-		 */
-		if (to_read) {
-			*cmd++ = (struct i2c_esp32_cmd) {
-				.opcode = I2C_ESP32_OP_READ,
-				.num_bytes = to_read,
+		if (rd_filled) {
+			cmd = (i2c_hw_cmd_t) {
+				.op_code = I2C_LL_CMD_READ,
+				.ack_en = false,
+				.ack_val = 0,
+				.byte_num = rd_filled
 			};
+
+			i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 		}
 
 		/* I2C master won't acknowledge the last byte read from the
-		 * slave device.  Divide the read command in two segments as
+		 * slave device. Divide the read command in two segments as
 		 * recommended by the ESP32 Technical Reference Manual.
 		 */
-		if (msg.len - to_read <= 1U) {
-			/* Read the last byte and explicitly ask for an
-			 * acknowledgment.
-			 */
-			*cmd++ = (struct i2c_esp32_cmd) {
-				.opcode = I2C_ESP32_OP_READ,
-				.num_bytes = 1,
-				.ack_val = true,
+		if (msg->len == 1) {
+			cmd = (i2c_hw_cmd_t)  {
+				.op_code = I2C_LL_CMD_READ,
+				.byte_num = 1,
+				.ack_val = 1,
 			};
-
-			/* Account for the `msg.len - 1` when clamping
-			 * transmission length to FIFO buffer size.
-			 */
-			to_read++;
-
-			if (msg.flags & I2C_MSG_STOP) {
-				wait_cmd = cmd;
-				*cmd++ = (struct i2c_esp32_cmd) {
-					.opcode = I2C_ESP32_OP_STOP
-				};
-			}
-		}
-		if (!wait_cmd) {
-			*cmd++ = (struct i2c_esp32_cmd) {
-				.opcode = I2C_ESP32_OP_END
-			};
+			msg->len = 0;
+			rd_filled++;
+			i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 		}
 
-		ret = i2c_esp32_transmit(dev, wait_cmd);
+		if (msg->len == 0) {
+			cmd = (i2c_hw_cmd_t)  {
+				.op_code = I2C_LL_CMD_STOP,
+				.byte_num = 0,
+				.ack_val = 0,
+				.ack_en = 0
+			};
+			i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
+		}
+
+		i2c_hal_write_cmd_reg(&data->hal, hw_end_cmd, data->cmd_idx++);
+		i2c_hal_enable_master_rx_it(&data->hal);
+
+		ret = i2c_esp32_transmit(dev);
 		if (ret < 0) {
+			LOG_ERR("I2C transfer error: %d", ret);
 			return ret;
 		}
 
-		for (i = 0U; i < to_read; i++) {
-			uint32_t v = sys_read32(I2C_DATA_APB_REG(config->index));
+		i2c_hal_read_rxfifo(&data->hal, msg->buf, rd_filled);
+		msg->buf += rd_filled;
 
-			*msg.buf++ = v & I2C_FIFO_RDATA;
-		}
-		msg.len -= to_read;
-
-		i2c_esp32_reset_fifo(config);
+		/* reset fifo read pointer */
+		data->cmd_idx = 0;
 	}
 
 	return 0;
 }
 
-static int i2c_esp32_write_msg(const struct device *dev, uint16_t addr,
-			       struct i2c_msg msg)
+static int IRAM_ATTR i2c_esp32_write_msg(const struct device *dev,
+		struct i2c_msg *msg, uint16_t addr)
 {
-	const struct i2c_esp32_config *config = dev->config;
-	volatile struct i2c_esp32_cmd *cmd =
-		(void *)I2C_COMD0_REG(config->index);
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+	uint8_t wr_filled = 0;
+	uint8_t *write_pr = NULL;
+	int ret = 0;
 
-	*cmd++ = (struct i2c_esp32_cmd) {
-		.opcode = I2C_ESP32_OP_RSTART
+	/* reset command index and set status as write operation */
+	data->cmd_idx = 0;
+	data->status = I2C_STATUS_WRITE;
+
+	i2c_hw_cmd_t cmd = {
+		.op_code = I2C_LL_CMD_WRITE,
+		.ack_en = true
 	};
 
-	cmd = i2c_esp32_write_addr(dev, cmd, &msg, addr);
+	i2c_hw_cmd_t hw_end_cmd = {
+		.op_code = I2C_LL_CMD_END,
+	};
 
-	for (; msg.len; cmd = (void *)I2C_COMD0_REG(config->index)) {
-		uint32_t to_send = MIN(I2C_ESP32_BUFFER_SIZE, msg.len);
-		uint32_t i;
-		int ret;
+	/* write restart command and address */
+	i2c_esp32_write_addr(dev, addr);
 
-		/* Copy data to TX fifo */
-		for (i = 0U; i < to_send; i++) {
-			sys_write32(*msg.buf++,
-				    I2C_DATA_APB_REG(config->index));
+	for (;;) {
+		wr_filled = (msg->len > SOC_I2C_FIFO_LEN) ? SOC_I2C_FIFO_LEN : msg->len;
+		write_pr = msg->buf;
+		msg->buf += wr_filled;
+		msg->len -= wr_filled;
+		cmd.byte_num = wr_filled;
+
+		if (wr_filled > 0) {
+			i2c_hal_write_txfifo(&data->hal, write_pr, wr_filled);
+			i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 		}
-		*cmd++ = (struct i2c_esp32_cmd) {
-			.opcode = I2C_ESP32_OP_WRITE,
-			.num_bytes = to_send,
-			.ack_en = true,
-		};
-		msg.len -= to_send;
 
-		if (!msg.len || (msg.flags & I2C_MSG_STOP)) {
-			*cmd = (struct i2c_esp32_cmd) {
-				.opcode = I2C_ESP32_OP_STOP
+		if (msg->len == 0 || (msg->flags & I2C_MSG_STOP)) {
+			cmd = (i2c_hw_cmd_t) {
+				.op_code = I2C_LL_CMD_STOP,
+				.ack_en = false,
+				.byte_num = 0
 			};
+
+			i2c_hal_write_cmd_reg(&data->hal, cmd, data->cmd_idx++);
 		} else {
-			*cmd = (struct i2c_esp32_cmd) {
-				.opcode = I2C_ESP32_OP_END
-			};
+			i2c_hal_write_cmd_reg(&data->hal, hw_end_cmd, data->cmd_idx++);
 		}
 
-		ret = i2c_esp32_transmit(dev, cmd);
+		i2c_hal_enable_master_tx_it(&data->hal);
+
+		ret = i2c_esp32_transmit(dev);
 		if (ret < 0) {
+			LOG_ERR("I2C transfer error: %d", ret);
 			return ret;
 		}
 
-		i2c_esp32_reset_fifo(config);
+		/* reset fifo write pointer */
+		data->cmd_idx = 0;
+
+		if (msg->len == 0) {
+			break;
+		}
 	}
 
 	return 0;
 }
 
-static int i2c_esp32_transfer(const struct device *dev, struct i2c_msg *msgs,
+static int IRAM_ATTR i2c_esp32_transfer(const struct device *dev, struct i2c_msg *msgs,
 			      uint8_t num_msgs, uint16_t addr)
 {
-	struct i2c_esp32_data *data = dev->data;
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
 	int ret = 0;
-	uint8_t i;
 
 	k_sem_take(&data->transfer_sem, K_FOREVER);
 
+	if (!num_msgs) {
+		return 0;
+	}
+
 	/* Mask out unused address bits, and make room for R/W bit */
 	addr &= BIT_MASK(data->dev_config & I2C_ADDR_10_BITS ? 10 : 7);
 	addr <<= 1;
 
-	for (i = 0U; i < num_msgs; i++) {
-		if ((msgs[i].flags & I2C_MSG_RW_MASK) == I2C_MSG_WRITE) {
-			ret = i2c_esp32_write_msg(dev, addr, msgs[i]);
+	for (; num_msgs > 0; num_msgs--, msgs++) {
+
+		if (data->status == I2C_STATUS_TIMEOUT || i2c_hal_is_bus_busy(&data->hal)) {
+			i2c_hw_fsm_reset(dev);
+		}
+
+		/* reset all fifo buffer before start */
+		i2c_esp32_reset_fifo(dev);
+
+		/* These two interrupts some times can not be cleared when the FSM gets stuck. */
+		/* So we disable them when these two interrupt occurs and re-enable them here. */
+		i2c_hal_disable_intr_mask(&data->hal, I2C_LL_INTR_MASK);
+		i2c_hal_clr_intsts_mask(&data->hal, I2C_LL_INTR_MASK);
+
+		if ((msgs->flags & I2C_MSG_RW_MASK) == I2C_MSG_READ) {
+			ret = i2c_esp32_read_msg(dev, msgs, addr);
 		} else {
-			ret = i2c_esp32_read_msg(dev, addr, msgs[i]);
+			ret = i2c_esp32_write_msg(dev, msgs, addr);
 		}
 
 		if (ret < 0) {
@@ -528,37 +548,29 @@
 	return ret;
 }
 
-static void i2c_esp32_isr(void *arg)
+static void IRAM_ATTR i2c_esp32_isr(void *arg)
 {
 	struct device *dev = (struct device *)arg;
-	const int fifo_give_mask = I2C_ACK_ERR_INT_ST |
-				   I2C_TIME_OUT_INT_ST |
-				   I2C_TRANS_COMPLETE_INT_ST |
-				   I2C_ARBITRATION_LOST_INT_ST;
-	const struct i2c_esp32_config *config = dev->config;
-	uint32_t status = sys_read32(I2C_INT_STATUS_REG(config->index));
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
+	i2c_intr_event_t evt_type = I2C_INTR_EVENT_ERR;
 
-	if (status & fifo_give_mask) {
-		struct i2c_esp32_data *data = dev->data;
-
-		/* Only give the semaphore if a watched interrupt happens.
-		 * Error checking is performed at the other side of the
-		 * semaphore, by reading the status register.
-		 */
-		if (status & I2C_ACK_ERR_INT_ST) {
-			data->err_status |= I2C_ACK_ERR_INT_ST;
-		}
-		if (status & I2C_ARBITRATION_LOST_INT_ST) {
-			data->err_status |= I2C_ARBITRATION_LOST_INT_ST;
-		}
-		if (status & I2C_TIME_OUT_INT_ST) {
-			data->err_status |= I2C_TIME_OUT_INT_ST;
-		}
-		k_sem_give(&data->fifo_sem);
+	if (data->status == I2C_STATUS_WRITE) {
+		i2c_hal_master_handle_tx_event(&data->hal, &evt_type);
+	} else if (data->status == I2C_STATUS_READ) {
+		i2c_hal_master_handle_rx_event(&data->hal, &evt_type);
 	}
 
-	/* Acknowledge all I2C interrupts */
-	sys_write32(~0, I2C_INT_CLR_REG(config->index));
+	if (evt_type == I2C_INTR_EVENT_NACK) {
+		data->status = I2C_STATUS_ACK_ERROR;
+	} else if (evt_type == I2C_INTR_EVENT_TOUT) {
+		data->status = I2C_STATUS_TIMEOUT;
+	} else if (evt_type == I2C_INTR_EVENT_ARBIT_LOST) {
+		data->status = I2C_STATUS_TIMEOUT;
+	} else if (evt_type == I2C_INTR_EVENT_TRANS_DONE) {
+		data->status = I2C_STATUS_DONE;
+	}
+
+	k_sem_give(&data->cmd_sem);
 }
 
 static int i2c_esp32_init(const struct device *dev);
@@ -566,93 +578,108 @@
 static const struct i2c_driver_api i2c_esp32_driver_api = {
 	.configure = i2c_esp32_configure,
 	.transfer = i2c_esp32_transfer,
+	.recover_bus = i2c_esp32_recover
 };
 
-#if DT_NODE_HAS_STATUS(DT_DRV_INST(0), okay)
-static const struct i2c_esp32_config i2c_esp32_config_0 = {
-	.index = 0,
-	.clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(0)),
-	.peripheral_id = (clock_control_subsys_t)DT_INST_CLOCKS_CELL(0, offset),
-	.sig = {
-		.sda_out = I2CEXT0_SDA_OUT_IDX,
-		.sda_in = I2CEXT0_SDA_IN_IDX,
-		.scl_out = I2CEXT0_SCL_OUT_IDX,
-		.scl_in = I2CEXT0_SCL_IN_IDX,
-	},
-	.pins = {
-		.scl = DT_INST_PROP(0, scl_pin),
-		.sda = DT_INST_PROP(0, sda_pin),
-	},
-	.mode = {
-		.tx_lsb_first =
-			IS_ENABLED(CONFIG_I2C_ESP32_0_TX_LSB_FIRST),
-		.rx_lsb_first =
-			IS_ENABLED(CONFIG_I2C_ESP32_0_RX_LSB_FIRST),
-	},
-	.irq_source = DT_IRQN(DT_NODELABEL(i2c0)),
-	.default_config = I2C_MODE_MASTER, /* FIXME: Zephyr don't support I2C_SLAVE_MODE */
-	.bitrate = DT_INST_PROP(0, clock_frequency),
-};
-
-static struct i2c_esp32_data i2c_esp32_data_0;
-
-DEVICE_DT_INST_DEFINE(0, &i2c_esp32_init, NULL,
-		    &i2c_esp32_data_0, &i2c_esp32_config_0,
-		    POST_KERNEL, CONFIG_I2C_INIT_PRIORITY,
-		    &i2c_esp32_driver_api);
-#endif /* DT_NODE_HAS_STATUS(DT_DRV_INST(0), okay) */
-
-#if DT_NODE_HAS_STATUS(DT_DRV_INST(1), okay)
-static const struct i2c_esp32_config i2c_esp32_config_1 = {
-	.index = 1,
-	.clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(1)),
-	.peripheral_id = (clock_control_subsys_t)DT_INST_CLOCKS_CELL(1, offset),
-	.sig = {
-		.sda_out = I2CEXT1_SDA_OUT_IDX,
-		.sda_in = I2CEXT1_SDA_IN_IDX,
-		.scl_out = I2CEXT1_SCL_OUT_IDX,
-		.scl_in = I2CEXT1_SCL_IN_IDX,
-	},
-	.pins = {
-		.scl = DT_INST_PROP(1, scl_pin),
-		.sda = DT_INST_PROP(1, sda_pin),
-	},
-	.mode = {
-		.tx_lsb_first =
-			IS_ENABLED(CONFIG_I2C_ESP32_1_TX_LSB_FIRST),
-		.rx_lsb_first =
-			IS_ENABLED(CONFIG_I2C_ESP32_1_RX_LSB_FIRST),
-	},
-	.irq_source = DT_IRQN(DT_NODELABEL(i2c1)),
-	.default_config = I2C_MODE_MASTER, /* FIXME: Zephyr don't support I2C_SLAVE_MODE */
-	.bitrate = DT_INST_PROP(1, clock_frequency),
-};
-
-static struct i2c_esp32_data i2c_esp32_data_1;
-
-DEVICE_DT_INST_DEFINE(1, &i2c_esp32_init, NULL,
-		    &i2c_esp32_data_1, &i2c_esp32_config_1,
-		    POST_KERNEL, CONFIG_I2C_INIT_PRIORITY,
-		    &i2c_esp32_driver_api);
-#endif /* DT_NODE_HAS_STATUS(DT_DRV_INST(1), okay) */
-
-static int i2c_esp32_init(const struct device *dev)
+static int IRAM_ATTR i2c_esp32_init(const struct device *dev)
 {
 	const struct i2c_esp32_config *config = dev->config;
-	struct i2c_esp32_data *data = dev->data;
-	uint32_t bitrate_cfg = i2c_map_dt_bitrate(config->bitrate);
+	struct i2c_esp32_data *data = (struct i2c_esp32_data *const)(dev)->data;
 
-	unsigned int key = irq_lock();
+	if (config->scl.gpio_name == NULL || config->sda.gpio_name == NULL) {
+		LOG_ERR("Failed to get GPIO device");
+		return -EINVAL;
+	}
 
-	k_sem_init(&data->fifo_sem, 1, 1);
-	k_sem_init(&data->transfer_sem, 1, 1);
+	data->scl_gpio = device_get_binding(config->scl.gpio_name);
+	if (!data->scl_gpio) {
+		LOG_ERR("Failed to get SCL GPIO device");
+		return -EINVAL;
+	}
 
-	/* Even if irq_enable() is called on config->irq.line, disable
-	 * interrupt sources in the I2C controller.
-	 */
-	sys_write32(0, I2C_INT_ENA_REG(config->index));
+	data->sda_gpio = device_get_binding(config->sda.gpio_name);
+	if (!data->sda_gpio) {
+		LOG_ERR("Failed to get SDA GPIO device");
+		return -EINVAL;
+	}
+
+	int ret = i2c_esp32_config_pin(dev);
+
+	if (ret < 0) {
+		LOG_ERR("Failed to configure I2C pins");
+		return -EINVAL;
+	}
+
+	clock_control_on(config->clock_dev, config->clock_subsys);
+
 	data->irq_line = esp_intr_alloc(config->irq_source, 0, i2c_esp32_isr, (void *)dev, NULL);
-	irq_unlock(key);
 
-	return i2c_esp32_configure(dev, config->default_config | bitrate_cfg);
+	return i2c_esp32_configure(dev, config->default_config);
 }
+
+#define GPIO0_NAME COND_CODE_1(DT_NODE_HAS_STATUS(DT_NODELABEL(gpio0), okay), \
+			     (DT_LABEL(DT_INST(0, espressif_esp32_gpio))), (NULL))
+#define GPIO1_NAME COND_CODE_1(DT_NODE_HAS_STATUS(DT_NODELABEL(gpio1), okay), \
+			     (DT_LABEL(DT_INST(1, espressif_esp32_gpio))), (NULL))
+
+#define DT_I2C_ESP32_GPIO_NAME(idx, pin) ( \
+	DT_INST_PROP(idx, pin) < 32 ? GPIO0_NAME : GPIO1_NAME)
+
+#define I2C_ESP32_FREQUENCY(bitrate)				       \
+	 (bitrate == I2C_BITRATE_STANDARD ? KHZ(100)	       \
+	: bitrate == I2C_BITRATE_FAST     ? KHZ(400)	       \
+	: bitrate == I2C_BITRATE_FAST_PLUS  ? MHZ(1) : 0)
+#define I2C_FREQUENCY(idx)						       \
+	I2C_ESP32_FREQUENCY(DT_INST_PROP(idx, clock_frequency))
+
+#define ESP32_I2C_INIT(idx)		\
+	static struct i2c_esp32_data i2c_esp32_data_##idx = {		  \
+		.hal = {	\
+			.dev = (i2c_dev_t *) DT_REG_ADDR(DT_NODELABEL(i2c##idx)),	\
+		},	\
+		.cmd_sem = Z_SEM_INITIALIZER(                            \
+			i2c_esp32_data_##idx.cmd_sem, 0, 1),                 \
+		.transfer_sem = Z_SEM_INITIALIZER(                        \
+			i2c_esp32_data_##idx.transfer_sem, 1, 1)		      \
+	};							\
+								\
+	static const struct i2c_esp32_config i2c_esp32_config_##idx = {	       \
+	.index = idx, \
+	.clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(idx)), \
+	.clock_subsys = (clock_control_subsys_t)DT_INST_CLOCKS_CELL(idx, offset), \
+	.scl = {	\
+		.gpio_name = DT_I2C_ESP32_GPIO_NAME(idx, scl_pin),	\
+		.sig_out = I2CEXT##idx##_SCL_OUT_IDX,	\
+		.sig_in = I2CEXT##idx##_SCL_IN_IDX,	\
+		.pin = DT_INST_PROP(idx, scl_pin), \
+	},	\
+	.sda = {	\
+		.gpio_name = DT_I2C_ESP32_GPIO_NAME(idx, sda_pin),	\
+		.sig_out = I2CEXT##idx##_SDA_OUT_IDX,	\
+		.sig_in = I2CEXT##idx##_SDA_IN_IDX,	\
+		.pin = DT_INST_PROP(idx, sda_pin), \
+	},	\
+	.mode = { \
+		.tx_lsb_first = DT_INST_PROP(idx, tx_lsb), \
+		.rx_lsb_first = DT_INST_PROP(idx, rx_lsb), \
+	}, \
+	.irq_source = ETS_I2C_EXT##idx##_INTR_SOURCE,	\
+	.bitrate = I2C_FREQUENCY(idx),	\
+	.default_config = I2C_MODE_MASTER,				\
+	};								       \
+	DEVICE_DT_DEFINE(DT_NODELABEL(i2c##idx),					       \
+		      &i2c_esp32_init,					       \
+		      NULL,				       \
+		      &i2c_esp32_data_##idx,				       \
+		      &i2c_esp32_config_##idx,				       \
+		      POST_KERNEL,					       \
+		      CONFIG_I2C_INIT_PRIORITY,	       \
+		      &i2c_esp32_driver_api); \
+
+#if DT_NODE_HAS_STATUS(DT_NODELABEL(i2c0), okay)
+ESP32_I2C_INIT(0);
+#endif
+
+#if DT_NODE_HAS_STATUS(DT_NODELABEL(i2c1), okay)
+ESP32_I2C_INIT(1);
+#endif
diff --git a/dts/bindings/i2c/espressif,esp32-i2c.yaml b/dts/bindings/i2c/espressif,esp32-i2c.yaml
index 30417127..31b149f 100644
--- a/dts/bindings/i2c/espressif,esp32-i2c.yaml
+++ b/dts/bindings/i2c/espressif,esp32-i2c.yaml
@@ -23,3 +23,13 @@
       type: int
       description: SCL pin
       required: true
+
+    tx-lsb:
+      type: boolean
+      required: false
+      description: Set I2C TX data as LSB
+
+    rx-lsb:
+      type: boolean
+      required: false
+      description: Set I2C RX data as LSB
diff --git a/dts/riscv/espressif/esp32c3.dtsi b/dts/riscv/espressif/esp32c3.dtsi
index c4e89e6..a5dbd7f 100644
--- a/dts/riscv/espressif/esp32c3.dtsi
+++ b/dts/riscv/espressif/esp32c3.dtsi
@@ -5,12 +5,14 @@
  */
 #include <mem.h>
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/i2c/i2c.h>
 #include <dt-bindings/interrupt-controller/esp-esp32c3-intmux.h>
 #include <dt-bindings/clock/esp32c3_clock.h>
 
 / {
 	#address-cells = <1>;
 	#size-cells = <1>;
+
 	chosen {
 		zephyr,entropy = &trng0;
 		zephyr,flash-controller = &flash;
@@ -98,6 +100,18 @@
 			ngpios = <32>;   /* 0..31 */
 		};
 
+		i2c0: i2c@60013000 {
+			compatible = "espressif,esp32-i2c";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <0x60013000 0x1000>;
+			interrupts = <I2C_EXT0_INTR_SOURCE>;
+			interrupt-parent = <&intc>;
+			label = "I2C_0";
+			clocks = <&rtc ESP32_I2C0_MODULE>;
+			status = "disabled";
+		};
+
 		uart0: uart@60000000 {
 			compatible = "espressif,esp32-uart";
 			reg = <0x60000000 0x400>;
diff --git a/dts/xtensa/espressif/esp32s2.dtsi b/dts/xtensa/espressif/esp32s2.dtsi
index 7a389d0..e85f38d 100644
--- a/dts/xtensa/espressif/esp32s2.dtsi
+++ b/dts/xtensa/espressif/esp32s2.dtsi
@@ -6,6 +6,7 @@
 #include <mem.h>
 #include <xtensa/xtensa.dtsi>
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/i2c/i2c.h>
 #include <dt-bindings/clock/esp32s2_clock.h>
 #include <dt-bindings/interrupt-controller/esp32s2-xtensa-intmux.h>
 
@@ -131,6 +132,30 @@
 			ngpios = <22>;   /* 32..53 */
 		};
 
+		i2c0: i2c@3f413000 {
+			compatible = "espressif,esp32-i2c";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <0x3f413000 0x1000>;
+			interrupts = <I2C_EXT0_INTR_SOURCE>;
+			interrupt-parent = <&intc>;
+			label = "I2C_0";
+			clocks = <&rtc ESP32_I2C0_MODULE>;
+			status = "disabled";
+		};
+
+		i2c1: i2c@3f427000 {
+			compatible = "espressif,esp32-i2c";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <0x3f427000 0x1000>;
+			interrupts = <I2C_EXT1_INTR_SOURCE>;
+			interrupt-parent = <&intc>;
+			label = "I2C_1";
+			clocks = <&rtc ESP32_I2C1_MODULE>;
+			status = "disabled";
+		};
+
 		timer0: counter@3f41f000 {
 			compatible = "espressif,esp32-timer";
 			reg = <0x3f41f000 DT_SIZE_K(4)>;
diff --git a/soc/riscv/esp32c3/soc.h b/soc/riscv/esp32c3/soc.h
index 9d79ed1..6b395ac 100644
--- a/soc/riscv/esp32c3/soc.h
+++ b/soc/riscv/esp32c3/soc.h
@@ -49,6 +49,11 @@
 extern void esp_rom_Cache_Resume_ICache(uint32_t autoload);
 extern int esp_rom_Cache_Invalidate_Addr(uint32_t addr, uint32_t size);
 extern spiflash_legacy_data_t esp_rom_spiflash_legacy_data;
+extern int esp_rom_gpio_matrix_in(uint32_t gpio, uint32_t signal_index,
+				    bool inverted);
+extern int esp_rom_gpio_matrix_out(uint32_t gpio, uint32_t signal_index,
+				     bool out_inverted,
+				     bool out_enabled_inverted);
 
 #endif /* _ASMLANGUAGE */