diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
index 9db0c5d..caf8eef 100644
--- a/drivers/st/ddr/stm32mp1_ddr.c
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -700,7 +700,7 @@
 
 static int board_ddr_power_init(enum ddr_type ddr_type)
 {
-	if (dt_check_pmic()) {
+	if (dt_pmic_status() > 0) {
 		return pmic_ddr_power_init(ddr_type);
 	}
 
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
index 2be7afe..ed88052 100644
--- a/drivers/st/i2c/stm32_i2c.c
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -8,10 +8,16 @@
 #include <stdbool.h>
 #include <stdlib.h>
 
-#include <arch_helpers.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_i2c.h>
 #include <lib/mmio.h>
+#include <lib/utils.h>
 
 /* STM32 I2C registers offsets */
 #define I2C_CR1			0x00U
@@ -26,50 +32,122 @@
 #define I2C_RXDR		0x24U
 #define I2C_TXDR		0x28U
 
-#define MAX_DELAY		0xFFFFFFFFU
-
-/* I2C TIMING clear register Mask */
-#define TIMING_CLEAR_MASK	0xF0FFFFFFU
-/* Timeout 25 ms */
-#define I2C_TIMEOUT_BUSY	25U
+#define TIMINGR_CLEAR_MASK	0xF0FFFFFFU
 
 #define MAX_NBYTE_SIZE		255U
 
-static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
-				    uint16_t dev_addr, uint16_t mem_addr,
-				    uint16_t mem_add_size, uint32_t timeout,
-				    uint32_t tick_start);
-static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-				   uint16_t mem_addr, uint16_t mem_add_size,
-				   uint32_t timeout, uint32_t tick_start);
+#define I2C_NSEC_PER_SEC	1000000000L
 
-/* Private functions to handle flags during polling transfer */
-static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
-			 uint8_t awaited_value, uint32_t timeout,
-			 uint32_t tick_start);
-static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint32_t timeout,
-			 uint32_t tick_start);
-static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint32_t timeout,
-			 uint32_t tick_start);
-static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint32_t timeout,
-			  uint32_t tick_start);
+/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
+#define I2C_TIMING			0x10D07DB5
 
-/* Private function to flush TXDR register */
-static void i2c_flush_txdr(struct i2c_handle_s *hi2c);
+static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
+{
+	hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+	hi2c->i2c_state = I2C_STATE_READY;
+}
 
-/* Private function to start, restart or stop a transfer */
-static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-				uint16_t size, uint32_t i2c_mode,
-				uint32_t request);
+/*
+ * @brief  Configure I2C Analog noise filter.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C peripheral.
+ * @param  analog_filter: New state of the Analog filter
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_config_analog_filter(struct i2c_handle_s *hi2c,
+				    uint32_t analog_filter)
+{
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	hi2c->lock = 1;
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	/* Disable the selected I2C peripheral */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	/* Reset I2Cx ANOFF bit */
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_ANFOFF);
+
+	/* Set analog filter bit*/
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, analog_filter);
+
+	/* Enable the selected I2C peripheral */
+	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+
+	hi2c->lock = 0;
+
+	return 0;
+}
+
+/*
+ * @brief  Get I2C setup information from the device tree and set pinctrl
+ *         configuration.
+ * @param  fdt: Pointer to the device tree
+ * @param  node: I2C node offset
+ * @param  init: Ref to the initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+				 struct stm32_i2c_init_s *init)
+{
+	const fdt32_t *cuint;
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL);
+	if (cuint == NULL) {
+		init->rise_time = STM32_I2C_RISE_TIME_DEFAULT;
+	} else {
+		init->rise_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL);
+	if (cuint == NULL) {
+		init->fall_time = STM32_I2C_FALL_TIME_DEFAULT;
+	} else {
+		init->fall_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "clock-frequency", NULL);
+	if (cuint == NULL) {
+		init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+	} else {
+		switch (fdt32_to_cpu(*cuint)) {
+		case STANDARD_RATE:
+			init->speed_mode = I2C_SPEED_STANDARD;
+			break;
+		case FAST_RATE:
+			init->speed_mode = I2C_SPEED_FAST;
+			break;
+		case FAST_PLUS_RATE:
+			init->speed_mode = I2C_SPEED_FAST_PLUS;
+			break;
+		default:
+			init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+			break;
+		}
+	}
+
+	return dt_set_pinctrl_config(node);
+}
 
 /*
  * @brief  Initialize the I2C device.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
  *               the configuration information for the specified I2C.
+ * @param  init_data: Initialization configuration structure
  * @retval 0 if OK, negative value else
  */
-int stm32_i2c_init(struct i2c_handle_s *hi2c)
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+		   struct stm32_i2c_init_s *init_data)
 {
+	int rc = 0;
+	uint32_t timing = I2C_TIMING;
+
 	if (hi2c == NULL) {
 		return -ENOENT;
 	}
@@ -80,34 +158,38 @@
 
 	hi2c->i2c_state = I2C_STATE_BUSY;
 
+	stm32mp_clk_enable(hi2c->clock);
+
 	/* Disable the selected I2C peripheral */
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
 
 	/* Configure I2Cx: Frequency range */
 	mmio_write_32(hi2c->i2c_base_addr + I2C_TIMINGR,
-		      hi2c->i2c_init.timing & TIMING_CLEAR_MASK);
+		      timing & TIMINGR_CLEAR_MASK);
 
 	/* Disable Own Address1 before set the Own Address1 configuration */
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_OAR1, I2C_OAR1_OA1EN);
 
 	/* Configure I2Cx: Own Address1 and ack own address1 mode */
-	if (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
 		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
-			      I2C_OAR1_OA1EN | hi2c->i2c_init.own_address1);
+			      I2C_OAR1_OA1EN | init_data->own_address1);
 	} else { /* I2C_ADDRESSINGMODE_10BIT */
 		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
 			      I2C_OAR1_OA1EN | I2C_OAR1_OA1MODE |
-			      hi2c->i2c_init.own_address1);
+			      init_data->own_address1);
 	}
 
+	mmio_write_32(hi2c->i2c_base_addr + I2C_CR2, 0);
+
 	/* Configure I2Cx: Addressing Master mode */
-	if (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
-		mmio_write_32(hi2c->i2c_base_addr + I2C_CR2, I2C_CR2_ADD10);
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
+		mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_CR2_ADD10);
 	}
 
 	/*
 	 * Enable the AUTOEND by default, and enable NACK
-	 * (should be disable only during Slave process)
+	 * (should be disabled only during Slave process).
 	 */
 	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
 			I2C_CR2_AUTOEND | I2C_CR2_NACK);
@@ -117,14 +199,14 @@
 
 	/* Configure I2Cx: Dual mode and Own Address2 */
 	mmio_write_32(hi2c->i2c_base_addr + I2C_OAR2,
-		      hi2c->i2c_init.dual_address_mode |
-		      hi2c->i2c_init.own_address2 |
-		      (hi2c->i2c_init.own_address2_masks << 8));
+		      init_data->dual_address_mode |
+		      init_data->own_address2 |
+		      (init_data->own_address2_masks << 8));
 
 	/* Configure I2Cx: Generalcall and NoStretch mode */
 	mmio_write_32(hi2c->i2c_base_addr + I2C_CR1,
-		      hi2c->i2c_init.general_call_mode |
-		      hi2c->i2c_init.no_stretch_mode);
+		      init_data->general_call_mode |
+		      init_data->no_stretch_mode);
 
 	/* Enable the selected I2C peripheral */
 	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
@@ -133,366 +215,207 @@
 	hi2c->i2c_state = I2C_STATE_READY;
 	hi2c->i2c_mode = I2C_MODE_NONE;
 
-	return 0;
+	rc = i2c_config_analog_filter(hi2c, init_data->analog_filter ?
+						I2C_ANALOGFILTER_ENABLE :
+						I2C_ANALOGFILTER_DISABLE);
+	if (rc != 0) {
+		ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
+		stm32mp_clk_disable(hi2c->clock);
+		return rc;
+	}
+
+	stm32mp_clk_disable(hi2c->clock);
+
+	return rc;
 }
 
 /*
- * @brief  Write an amount of data in blocking mode to a specific memory address
- * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
- *               the configuration information for the specified I2C.
- * @param  dev_addr: Target device address
- * @param  mem_addr: Internal memory address
- * @param  mem_add_size: size of internal memory address
- * @param  p_data: Pointer to data buffer
- * @param  size: Amount of data to be sent
- * @param  timeout: timeout duration
- * @retval 0 if OK, negative value else
+ * @brief  I2C Tx data register flush process.
+ * @param  hi2c: I2C handle
+ * @retval None
  */
-int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-			uint16_t mem_addr, uint16_t mem_add_size,
-			uint8_t *p_data, uint16_t size, uint32_t timeout)
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c)
 {
-	uint32_t tickstart;
-
-	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
-		return -EBUSY;
+	/*
+	 * If a pending TXIS flag is set,
+	 * write a dummy data in TXDR to clear it.
+	 */
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXIS) !=
+	    0U) {
+		mmio_write_32(hi2c->i2c_base_addr + I2C_TXDR, 0);
 	}
 
-	if ((p_data == NULL) || (size == 0U)) {
-		return -EINVAL;
+	/* Flush TX register if not empty */
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXE) ==
+	    0U) {
+		mmio_setbits_32(hi2c->i2c_base_addr + I2C_ISR,
+				I2C_FLAG_TXE);
 	}
-
-	hi2c->lock = 1;
+}
 
-	tickstart = (uint32_t)read_cntpct_el0();
+/*
+ * @brief  This function handles I2C Communication timeout.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  flag: Specifies the I2C flag to check
+ * @param  awaited_value: The awaited bit value for the flag (0 or 1)
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value, uint64_t timeout_ref)
+{
+	for ( ; ; ) {
+		uint32_t isr = mmio_read_32(hi2c->i2c_base_addr + I2C_ISR);
 
-	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, I2C_TIMEOUT_BUSY,
-			  tickstart) != 0) {
-		return -EIO;
-	}
+		if (!!(isr & flag) != !!awaited_value) {
+			return 0;
+		}
 
-	hi2c->i2c_state     = I2C_STATE_BUSY_TX;
-	hi2c->i2c_mode      = I2C_MODE_MEM;
-	hi2c->i2c_err = I2C_ERROR_NONE;
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
 
-	hi2c->p_buff  = p_data;
-	hi2c->xfer_count = size;
+			return -EIO;
+		}
+	}
+}
 
-	/* Send Slave Address and Memory Address */
-	if (i2c_request_memory_write(hi2c, dev_addr, mem_addr, mem_add_size,
-				     timeout, tickstart) != 0) {
-		hi2c->lock = 0;
-		return -EIO;
+/*
+ * @brief  This function handles Acknowledge failed detection during
+ *	   an I2C Communication.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_AF) == 0U) {
+		return 0;
 	}
 
 	/*
-	 * Set NBYTES to write and reload
-	 * if hi2c->xfer_count > MAX_NBYTE_SIZE
+	 * Wait until STOP Flag is reset.
+	 * AutoEnd should be initiate after AF.
 	 */
-	if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
-		hi2c->xfer_size = MAX_NBYTE_SIZE;
-		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
-				    I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
-	} else {
-		hi2c->xfer_size = hi2c->xfer_count;
-		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
-				    I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
-	}
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		I2C_FLAG_STOPF) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
 
-	do {
-		if (i2c_wait_txis(hi2c, timeout, tickstart) != 0) {
 			return -EIO;
 		}
-
-		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR, *hi2c->p_buff);
-		hi2c->p_buff++;
-		hi2c->xfer_count--;
-		hi2c->xfer_size--;
-
-		if ((hi2c->xfer_count != 0U) && (hi2c->xfer_size == 0U)) {
-			/* Wait until TCR flag is set */
-			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout,
-					  tickstart) != 0) {
-				return -EIO;
-		}
-
-			if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
-				hi2c->xfer_size = MAX_NBYTE_SIZE;
-				i2c_transfer_config(hi2c, dev_addr,
-						    hi2c->xfer_size,
-						    I2C_RELOAD_MODE,
-						    I2C_NO_STARTSTOP);
-			} else {
-				hi2c->xfer_size = hi2c->xfer_count;
-				i2c_transfer_config(hi2c, dev_addr,
-						    hi2c->xfer_size,
-						    I2C_AUTOEND_MODE,
-						    I2C_NO_STARTSTOP);
-			}
-		}
-
-	} while (hi2c->xfer_count > 0U);
-
-	/*
-	 * No need to Check TC flag, with AUTOEND mode the stop
-	 * is automatically generated.
-	 * Wait until STOPF flag is reset.
-	 */
-	if (i2c_wait_stop(hi2c, timeout, tickstart) != 0) {
-		return -EIO;
 	}
 
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
 	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
 
+	i2c_flush_txdr(hi2c);
+
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
 
+	hi2c->i2c_err |= I2C_ERROR_AF;
 	hi2c->i2c_state = I2C_STATE_READY;
-	hi2c->i2c_mode  = I2C_MODE_NONE;
+	hi2c->i2c_mode = I2C_MODE_NONE;
 
 	hi2c->lock = 0;
 
-	return 0;
+	return -EIO;
 }
 
 /*
- * @brief  Read an amount of data in blocking mode from a specific memory
- *	   address
+ * @brief  This function handles I2C Communication timeout for specific usage
+ *	   of TXIS flag.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
  *               the configuration information for the specified I2C.
- * @param  dev_addr: Target device address
- * @param  mem_addr: Internal memory address
- * @param  mem_add_size: size of internal memory address
- * @param  p_data: Pointer to data buffer
- * @param  size: Amount of data to be sent
- * @param  timeout: timeout duration
+ * @param  timeout_ref: Reference to target timeout
  * @retval 0 if OK, negative value else
  */
-int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-		       uint16_t mem_addr, uint16_t mem_add_size,
-		       uint8_t *p_data, uint16_t size, uint32_t timeout)
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
 {
-	uint32_t tickstart;
-
-	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
-		return -EBUSY;
-	}
-
-	if ((p_data == NULL) || (size == 0U)) {
-		return  -EINVAL;
-	}
-
-	hi2c->lock = 1;
-
-	tickstart = (uint32_t)read_cntpct_el0();
-
-	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, I2C_TIMEOUT_BUSY,
-			  tickstart) != 0) {
-		return -EIO;
-	}
-
-	hi2c->i2c_state     = I2C_STATE_BUSY_RX;
-	hi2c->i2c_mode      = I2C_MODE_MEM;
-	hi2c->i2c_err = I2C_ERROR_NONE;
-
-	hi2c->p_buff  = p_data;
-	hi2c->xfer_count = size;
-
-	/* Send Slave Address and Memory Address */
-	if (i2c_request_memory_read(hi2c, dev_addr, mem_addr, mem_add_size,
-				    timeout, tickstart) != 0) {
-		hi2c->lock = 0;
-		return -EIO;
-	}
-
-	/*
-	 * Send Slave Address.
-	 * Set NBYTES to write and reload if hi2c->xfer_count > MAX_NBYTE_SIZE
-	 * and generate RESTART.
-	 */
-	if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
-		hi2c->xfer_size = MAX_NBYTE_SIZE;
-		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
-				    I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
-	} else {
-		hi2c->xfer_size = hi2c->xfer_count;
-		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
-				    I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
-	}
-
-	do {
-		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, timeout,
-				  tickstart) != 0) {
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		I2C_FLAG_TXIS) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
 			return -EIO;
 		}
 
-		*hi2c->p_buff = mmio_read_8(hi2c->i2c_base_addr + I2C_RXDR);
-		hi2c->p_buff++;
-		hi2c->xfer_size--;
-		hi2c->xfer_count--;
-
-		if ((hi2c->xfer_count != 0U) && (hi2c->xfer_size == 0U)) {
-			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout,
-					  tickstart) != 0) {
-				return -EIO;
-			}
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
 
-			if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
-				hi2c->xfer_size = MAX_NBYTE_SIZE;
-				i2c_transfer_config(hi2c, dev_addr,
-						    hi2c->xfer_size,
-						    I2C_RELOAD_MODE,
-						    I2C_NO_STARTSTOP);
-			} else {
-				hi2c->xfer_size = hi2c->xfer_count;
-				i2c_transfer_config(hi2c, dev_addr,
-						    hi2c->xfer_size,
-						    I2C_AUTOEND_MODE,
-						    I2C_NO_STARTSTOP);
-			}
+			return -EIO;
 		}
-	} while (hi2c->xfer_count > 0U);
-
-	/*
-	 * No need to Check TC flag, with AUTOEND mode the stop
-	 * is automatically generated
-	 * Wait until STOPF flag is reset
-	 */
-	if (i2c_wait_stop(hi2c, timeout, tickstart) != 0) {
-		return -EIO;
 	}
 
-	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
-
-	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
-
-	hi2c->i2c_state = I2C_STATE_READY;
-	hi2c->i2c_mode  = I2C_MODE_NONE;
-
-	hi2c->lock = 0;
-
 	return 0;
 }
 
 /*
- * @brief  Checks if target device is ready for communication.
- * @note   This function is used with Memory devices
+ * @brief  This function handles I2C Communication timeout for specific
+ *	   usage of STOP flag.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
  *               the configuration information for the specified I2C.
- * @param  dev_addr: Target device address
- * @param  trials: Number of trials
- * @param  timeout: timeout duration
+ * @param  timeout_ref: Reference to target timeout
  * @retval 0 if OK, negative value else
  */
-int stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
-			      uint16_t dev_addr, uint32_t trials,
-			      uint32_t timeout)
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
 {
-	uint32_t i2c_trials = 0U;
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		 I2C_FLAG_STOPF) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
+			return -EIO;
+		}
 
-	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
-		return -EBUSY;
-	}
+		if (timeout_elapsed(timeout_ref)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
 
-	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_BUSY) !=
-	    0U) {
-		return -EBUSY;
-	}
-
-	hi2c->lock = 1;
-
-	hi2c->i2c_state = I2C_STATE_BUSY;
-	hi2c->i2c_err = I2C_ERROR_NONE;
-
-	do {
-		uint32_t tickstart;
-
-		/* Generate Start */
-		if (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
-			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
-				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
-				       I2C_CR2_START | I2C_CR2_AUTOEND) &
-				       ~I2C_CR2_RD_WRN);
-		} else {
-			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
-				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
-				       I2C_CR2_START | I2C_CR2_ADD10) &
-				      ~I2C_CR2_RD_WRN);
-		}
-
-		/*
-		 * No need to Check TC flag, with AUTOEND mode the stop
-		 * is automatically generated
-		 * Wait until STOPF flag is set or a NACK flag is set
-		 */
-		tickstart = (uint32_t)read_cntpct_el0();
-		while (((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-			 (I2C_FLAG_STOPF | I2C_FLAG_AF)) == 0U) &&
-		       (hi2c->i2c_state != I2C_STATE_TIMEOUT)) {
-			if (timeout != MAX_DELAY) {
-				if ((((uint32_t)read_cntpct_el0() - tickstart) >
-				     timeout) || (timeout == 0U)) {
-					hi2c->i2c_state = I2C_STATE_READY;
-
-					hi2c->i2c_err |=
-						I2C_ERROR_TIMEOUT;
-
-					hi2c->lock = 0;
-
-					return -EIO;
-				}
-			}
-		}
-
-		/* Check if the NACKF flag has not been set */
-		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-		     I2C_FLAG_AF) == 0U) {
-			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout,
-					  tickstart) != 0) {
-				return -EIO;
-			}
-
-			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
-				      I2C_FLAG_STOPF);
-
-			hi2c->i2c_state = I2C_STATE_READY;
-
-			hi2c->lock = 0;
-
-			return 0;
-		}
-
-		if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout,
-				  tickstart) != 0) {
 			return -EIO;
 		}
-
-		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
-
-		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
-
-		if (i2c_trials == trials) {
-			mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
-					I2C_CR2_STOP);
-
-			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout,
-					  tickstart) != 0) {
-				return -EIO;
-			}
-
-			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
-				      I2C_FLAG_STOPF);
-		}
+	}
 
-		i2c_trials++;
-	} while (i2c_trials < trials);
+	return 0;
+}
 
-	hi2c->i2c_state = I2C_STATE_READY;
+/*
+ * @brief  Handles I2Cx communication when starting transfer or during transfer
+ *	   (TC or TCR flag are set).
+ * @param  hi2c: I2C handle
+ * @param  dev_addr: Specifies the slave address to be programmed
+ * @param  size: Specifies the number of bytes to be programmed.
+ *   This parameter must be a value between 0 and 255.
+ * @param  i2c_mode: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_RELOAD_MODE: Enable Reload mode.
+ *     @arg @ref I2C_AUTOEND_MODE: Enable Automatic end mode.
+ *     @arg @ref I2C_SOFTEND_MODE: Enable Software end mode.
+ * @param  request: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_NO_STARTSTOP: Don't Generate stop and start condition.
+ *     @arg @ref I2C_GENERATE_STOP: Generate stop condition
+ *                                  (size should be set to 0).
+ *     @arg @ref I2C_GENERATE_START_READ: Generate Restart for read request.
+ *     @arg @ref I2C_GENERATE_START_WRITE: Generate Restart for write request.
+ * @retval None
+ */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				uint16_t size, uint32_t i2c_mode,
+				uint32_t request)
+{
+	uint32_t clr_value, set_value;
 
-	hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+	clr_value = (I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD |
+		     I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_STOP) |
+		(I2C_CR2_RD_WRN & (request >> (31U - I2C_CR2_RD_WRN_OFFSET)));
 
-	hi2c->lock = 0;
+	set_value = ((uint32_t)dev_addr & I2C_CR2_SADD) |
+		(((uint32_t)size << I2C_CR2_NBYTES_OFFSET) & I2C_CR2_NBYTES) |
+		i2c_mode | request;
 
-	return -EIO;
+	mmio_clrsetbits_32(hi2c->i2c_base_addr + I2C_CR2, clr_value, set_value);
 }
 
 /*
@@ -502,20 +425,18 @@
  *               the configuration information for the specified I2C.
  * @param  dev_addr: Target device address
  * @param  mem_addr: Internal memory address
- * @param  mem_add_size: size of internal memory address
- * @param  timeout: timeout duration
- * @param  tick_start Tick start value
+ * @param  mem_add_size: Size of internal memory address
+ * @param  timeout_ref: Reference to target timeout
  * @retval 0 if OK, negative value else
  */
 static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
 				    uint16_t dev_addr, uint16_t mem_addr,
-				    uint16_t mem_add_size, uint32_t timeout,
-				    uint32_t tick_start)
+				    uint16_t mem_add_size, uint64_t timeout_ref)
 {
 	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_RELOAD_MODE,
 			    I2C_GENERATE_START_WRITE);
 
-	if (i2c_wait_txis(hi2c, timeout, tick_start) != 0) {
+	if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
 		return -EIO;
 	}
 
@@ -528,8 +449,7 @@
 		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
 			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
 
-		/* Wait until TXIS flag is set */
-		if (i2c_wait_txis(hi2c, timeout, tick_start) != 0) {
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
 			return -EIO;
 		}
 
@@ -538,8 +458,7 @@
 			     (uint8_t)(mem_addr & 0x00FFU));
 	}
 
-	if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout, tick_start) !=
-	    0) {
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout_ref) != 0) {
 		return -EIO;
 	}
 
@@ -553,19 +472,18 @@
  *               the configuration information for the specified I2C.
  * @param  dev_addr: Target device address
  * @param  mem_addr: Internal memory address
- * @param  mem_add_size: size of internal memory address
- * @param  timeout: timeout duration
- * @param  tick_start Tick start value
+ * @param  mem_add_size: Size of internal memory address
+ * @param  timeout_ref: Reference to target timeout
  * @retval 0 if OK, negative value else
  */
 static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 				   uint16_t mem_addr, uint16_t mem_add_size,
-				   uint32_t timeout, uint32_t tick_start)
+				   uint64_t timeout_ref)
 {
 	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_SOFTEND_MODE,
 			    I2C_GENERATE_START_WRITE);
 
-	if (i2c_wait_txis(hi2c, timeout, tick_start) != 0) {
+	if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
 		return -EIO;
 	}
 
@@ -578,8 +496,7 @@
 		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
 			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
 
-		/* Wait until TXIS flag is set */
-		if (i2c_wait_txis(hi2c, timeout, tick_start) != 0) {
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
 			return -EIO;
 		}
 
@@ -588,265 +505,477 @@
 			     (uint8_t)(mem_addr & 0x00FFU));
 	}
 
-	if (i2c_wait_flag(hi2c, I2C_FLAG_TC, 0, timeout, tick_start) != 0) {
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TC, 0, timeout_ref) != 0) {
 		return -EIO;
 	}
 
 	return 0;
 }
-
 /*
- * @brief  I2C Tx data register flush process.
- * @param  hi2c: I2C handle.
- * @retval None
+ * @brief  Generic function to write an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
+ * @retval 0 if OK, negative value else
  */
-static void i2c_flush_txdr(struct i2c_handle_s *hi2c)
+static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		     uint16_t mem_addr, uint16_t mem_add_size,
+		     uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		     enum i2c_mode_e mode)
 {
-	/*
-	 * If a pending TXIS flag is set,
-	 * write a dummy data in TXDR to clear it.
-	 */
-	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXIS) !=
-	    0U) {
-		mmio_write_32(hi2c->i2c_base_addr + I2C_TXDR, 0);
+	uint64_t timeout_ref;
+	int rc = -EIO;
+	uint8_t *p_buff = p_data;
+	uint32_t xfer_size;
+	uint32_t xfer_count = size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
 	}
 
-	/* Flush TX register if not empty */
-	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXE) ==
-	    0U) {
-		mmio_setbits_32(hi2c->i2c_base_addr + I2C_ISR,
-				I2C_FLAG_TXE);
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
 	}
-}
 
-/*
- * @brief  This function handles I2C Communication timeout.
- * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
- *               the configuration information for the specified I2C.
- * @param  flag: Specifies the I2C flag to check.
- * @param  awaited_value: The awaited bit value for the flag (0 or 1).
- * @param  timeout: timeout duration
- * @param  tick_start: Tick start value
- * @retval 0 if OK, negative value else
- */
-static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
-			 uint8_t awaited_value, uint32_t timeout,
-			 uint32_t tick_start)
-{
-	uint8_t flag_check;
+	if ((p_data == NULL) || (size == 0U)) {
+		return -EINVAL;
+	}
 
-	do {
-		flag_check = ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-			       flag) == flag) ? 1U : 0U;
+	stm32mp_clk_enable(hi2c->clock);
+
+	hi2c->lock = 1;
 
-		if (timeout != MAX_DELAY) {
-			if ((((uint32_t)read_cntpct_el0() - tick_start) >
-			     timeout) || (timeout == 0U)) {
-				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
-				hi2c->i2c_state = I2C_STATE_READY;
-				hi2c->i2c_mode = I2C_MODE_NONE;
+	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+		goto bail;
+	}
 
-				hi2c->lock = 0;
-				return -EIO;
-			}
+	hi2c->i2c_state = I2C_STATE_BUSY_TX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	timeout_ref = timeout_init_us(timeout_ms * 1000);
+
+	if (mode == I2C_MODE_MEM) {
+		/* In Memory Mode, Send Slave Address and Memory Address */
+		if (i2c_request_memory_write(hi2c, dev_addr, mem_addr,
+					     mem_add_size, timeout_ref) != 0) {
+			goto bail;
 		}
-	} while (flag_check == awaited_value);
 
-	return 0;
-}
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
+		}
+	} else {
+		/* In Master Mode, Send Slave Address */
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE,
+					    I2C_GENERATE_START_WRITE);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE,
+					    I2C_GENERATE_START_WRITE);
+		}
+	}
 
-/*
- * @brief  This function handles I2C Communication timeout for specific usage
- *	   of TXIS flag.
- * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
- *               the configuration information for the specified I2C.
- * @param  timeout: timeout duration
- * @param  tick_start: Tick start value
- * @retval 0 if OK, negative value else
- */
-static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint32_t timeout,
-			 uint32_t tick_start)
-{
-	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-		I2C_FLAG_TXIS) == 0U) {
-		if (i2c_ack_failed(hi2c, timeout, tick_start) != 0) {
-			return -EIO;
+	do {
+		if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+			goto bail;
 		}
 
-		if (timeout != MAX_DELAY) {
-			if ((((uint32_t)read_cntpct_el0() - tick_start) >
-			     timeout) || (timeout == 0U)) {
-				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
-				hi2c->i2c_state = I2C_STATE_READY;
-				hi2c->i2c_mode = I2C_MODE_NONE;
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR, *p_buff);
+		p_buff++;
+		xfer_count--;
+		xfer_size--;
 
-				hi2c->lock = 0;
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			/* Wait until TCR flag is set */
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
 
-				return -EIO;
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
 			}
 		}
+
+	} while (xfer_count > 0U);
+
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+		goto bail;
 	}
 
-	return 0;
+	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode  = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	stm32mp_clk_disable(hi2c->clock);
+
+	return rc;
 }
 
 /*
- * @brief  This function handles I2C Communication timeout for specific
- *	   usage of STOP flag.
+ * @brief  Write an amount of data in blocking mode to a specific memory
+ *         address.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
  *               the configuration information for the specified I2C.
- * @param  timeout: timeout duration
- * @param  tick_start: Tick start value
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
  * @retval 0 if OK, negative value else
  */
-static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint32_t timeout,
-			 uint32_t tick_start)
+int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			uint16_t mem_addr, uint16_t mem_add_size,
+			uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
 {
-	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-		 I2C_FLAG_STOPF) == 0U) {
-		if (i2c_ack_failed(hi2c, timeout, tick_start) != 0) {
-			return -EIO;
-		}
-
-		if ((((uint32_t)read_cntpct_el0() - tick_start) > timeout) ||
-		    (timeout == 0U)) {
-			hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
-			hi2c->i2c_state = I2C_STATE_READY;
-			hi2c->i2c_mode = I2C_MODE_NONE;
-
-			hi2c->lock = 0;
-
-			return -EIO;
-		}
-	}
+	return i2c_write(hi2c, dev_addr, mem_addr, mem_add_size,
+			 p_data, size, timeout_ms, I2C_MODE_MEM);
+}
 
-	return 0;
+/*
+ * @brief  Transmits in master mode an amount of data in blocking mode.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      uint8_t *p_data, uint16_t size,
+			      uint32_t timeout_ms)
+{
+	return i2c_write(hi2c, dev_addr, 0, 0,
+			 p_data, size, timeout_ms, I2C_MODE_MASTER);
 }
 
 /*
- * @brief  This function handles Acknowledge failed detection during
- *	   an I2C Communication.
+ * @brief  Generic function to read an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
  *               the configuration information for the specified I2C.
- * @param  timeout: timeout duration
- * @param  tick_start: Tick start value
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
  * @retval 0 if OK, negative value else
  */
-static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint32_t timeout,
-			  uint32_t tick_start)
+static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		    uint16_t mem_addr, uint16_t mem_add_size,
+		    uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		    enum i2c_mode_e mode)
 {
-	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_AF) == 0U) {
-		return 0;
+	uint64_t timeout_ref;
+	int rc = -EIO;
+	uint8_t *p_buff = p_data;
+	uint32_t xfer_count = size;
+	uint32_t xfer_size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
 	}
 
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return  -EINVAL;
+	}
+
+	stm32mp_clk_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY_RX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	if (mode == I2C_MODE_MEM) {
+		/* Send Memory Address */
+		if (i2c_request_memory_read(hi2c, dev_addr, mem_addr,
+					    mem_add_size, timeout_ref) != 0) {
+			goto bail;
+		}
+	}
+
 	/*
-	 * Wait until STOP Flag is reset.
-	 * AutoEnd should be initiate after AF.
+	 * Send Slave Address.
+	 * Set NBYTES to write and reload if xfer_count > MAX_NBYTE_SIZE
+	 * and generate RESTART.
 	 */
-	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
-		I2C_FLAG_STOPF) == 0U) {
-		if (timeout != MAX_DELAY) {
-			if ((((uint32_t)read_cntpct_el0() - tick_start) >
-			     timeout) || (timeout == 0U)) {
-				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
-				hi2c->i2c_state = I2C_STATE_READY;
-				hi2c->i2c_mode = I2C_MODE_NONE;
+	if (xfer_count > MAX_NBYTE_SIZE) {
+		xfer_size = MAX_NBYTE_SIZE;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
+	} else {
+		xfer_size = xfer_count;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
+	}
+
+	do {
+		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, timeout_ref) != 0) {
+			goto bail;
+		}
 
-				hi2c->lock = 0;
+		*p_buff = mmio_read_8(hi2c->i2c_base_addr + I2C_RXDR);
+		p_buff++;
+		xfer_size--;
+		xfer_count--;
 
-				return -EIO;
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
 			}
 		}
-	}
+	} while (xfer_count > 0U);
 
-	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+		goto bail;
+	}
 
 	mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
 
-	i2c_flush_txdr(hi2c);
-
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
 
-	hi2c->i2c_err |= I2C_ERROR_AF;
 	hi2c->i2c_state = I2C_STATE_READY;
 	hi2c->i2c_mode = I2C_MODE_NONE;
 
+	rc = 0;
+
+bail:
 	hi2c->lock = 0;
+	stm32mp_clk_disable(hi2c->clock);
 
-	return -EIO;
+	return rc;
 }
 
 /*
- * @brief  Handles I2Cx communication when starting transfer or during transfer
- *	   (TC or TCR flag are set).
- * @param  hi2c: I2C handle.
- * @param  dev_addr: Specifies the slave address to be programmed.
- * @param  size: Specifies the number of bytes to be programmed.
- *   This parameter must be a value between 0 and 255.
- * @param  i2c_mode: New state of the I2C START condition generation.
- *   This parameter can be one of the following values:
- *     @arg @ref I2C_RELOAD_MODE: Enable Reload mode .
- *     @arg @ref I2C_AUTOEND_MODE: Enable Automatic end mode.
- *     @arg @ref I2C_SOFTEND_MODE: Enable Software end mode.
- * @param  request: New state of the I2C START condition generation.
- *   This parameter can be one of the following values:
- *     @arg @ref I2C_NO_STARTSTOP: Don't Generate stop and start condition.
- *     @arg @ref I2C_GENERATE_STOP: Generate stop condition
- *                                  (size should be set to 0).
- *     @arg @ref I2C_GENERATE_START_READ: Generate Restart for read request.
- *     @arg @ref I2C_GENERATE_START_WRITE: Generate Restart for write request.
- * @retval None
+ * @brief  Read an amount of data in blocking mode from a specific memory
+ *	   address.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
  */
-static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-				uint16_t size, uint32_t i2c_mode,
-				uint32_t request)
+int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		       uint16_t mem_addr, uint16_t mem_add_size,
+		       uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
 {
-	uint32_t clr_value, set_value;
-
-	clr_value = (I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD |
-		     I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_STOP) |
-		(I2C_CR2_RD_WRN & (request >> (31U - I2C_CR2_RD_WRN_OFFSET)));
-
-	set_value = ((uint32_t)dev_addr & I2C_CR2_SADD) |
-		(((uint32_t)size << I2C_CR2_NBYTES_OFFSET) & I2C_CR2_NBYTES) |
-		i2c_mode | request;
-
-	mmio_clrsetbits_32(hi2c->i2c_base_addr + I2C_CR2, clr_value, set_value);
+	return i2c_read(hi2c, dev_addr, mem_addr, mem_add_size,
+			p_data, size, timeout_ms, I2C_MODE_MEM);
 }
 
 /*
- * @brief  Configure I2C Analog noise filter.
+ * @brief  Receives in master mode an amount of data in blocking mode.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
- *               the configuration information for the specified I2Cx peripheral
- * @param  analog_filter: New state of the Analog filter.
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
  * @retval 0 if OK, negative value else
  */
-int stm32_i2c_config_analog_filter(struct i2c_handle_s *hi2c,
-				   uint32_t analog_filter)
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			     uint8_t *p_data, uint16_t size,
+			     uint32_t timeout_ms)
 {
+	return i2c_read(hi2c, dev_addr, 0, 0,
+			p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief  Checks if target device is ready for communication.
+ * @note   This function is used with Memory devices
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  trials: Number of trials
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval True if device is ready, false else
+ */
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
+			       uint16_t dev_addr, uint32_t trials,
+			       uint32_t timeout_ms)
+{
+	uint32_t i2c_trials = 0U;
+	bool rc = false;
+
 	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
-		return -EBUSY;
+		return rc;
 	}
 
+	stm32mp_clk_enable(hi2c->clock);
+
 	hi2c->lock = 1;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_BUSY) !=
+	    0U) {
+		goto bail;
+	}
 
 	hi2c->i2c_state = I2C_STATE_BUSY;
+	hi2c->i2c_err = I2C_ERROR_NONE;
 
-	/* Disable the selected I2C peripheral */
-	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+	do {
+		uint64_t timeout_ref;
 
-	/* Reset I2Cx ANOFF bit */
-	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_ANFOFF);
+		/* Generate Start */
+		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_OAR1) &
+		     I2C_OAR1_OA1MODE) == 0) {
+			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_AUTOEND) &
+				      ~I2C_CR2_RD_WRN);
+		} else {
+			mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_ADD10) &
+				      ~I2C_CR2_RD_WRN);
+		}
 
-	/* Set analog filter bit*/
-	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, analog_filter);
+		/*
+		 * No need to Check TC flag, with AUTOEND mode the stop
+		 * is automatically generated.
+		 * Wait until STOPF flag is set or a NACK flag is set.
+		 */
+		timeout_ref = timeout_init_us(timeout_ms * 1000);
+		do {
+			if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+			     (I2C_FLAG_STOPF | I2C_FLAG_AF)) != 0U) {
+				break;
+			}
 
-	/* Enable the selected I2C peripheral */
-	mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+			if (timeout_elapsed(timeout_ref)) {
+				notif_i2c_timeout(hi2c);
+				goto bail;
+			}
+		} while (true);
 
-	hi2c->i2c_state = I2C_STATE_READY;
+		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		     I2C_FLAG_AF) == 0U) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
 
+			hi2c->i2c_state = I2C_STATE_READY;
+
+			rc = true;
+			goto bail;
+		}
+
+		if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout_ref) != 0) {
+			goto bail;
+		}
+
+		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
+		mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+		if (i2c_trials == trials) {
+			mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
+					I2C_CR2_STOP);
+
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+					  timeout_ref) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
+		}
+
+		i2c_trials++;
+	} while (i2c_trials < trials);
+
+	notif_i2c_timeout(hi2c);
+
+bail:
 	hi2c->lock = 0;
+	stm32mp_clk_disable(hi2c->clock);
 
-	return 0;
+	return rc;
 }
+
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
index 52f712f..6fe51f4 100644
--- a/drivers/st/pmic/stm32mp_pmic.c
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -5,7 +5,6 @@
  */
 
 #include <errno.h>
-#include <stdbool.h>
 
 #include <libfdt.h>
 
@@ -13,19 +12,12 @@
 
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <drivers/st/stm32_i2c.h>
 #include <drivers/st/stm32mp_pmic.h>
-#include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stpmic1.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
 
-/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
-#define I2C_TIMING			0x10D07DB5
-
-#define I2C_TIMEOUT			0xFFFFF
-
-#define MASK_RESET_BUCK3		BIT(2)
-
 #define STPMIC1_LDO12356_OUTPUT_MASK	(uint8_t)(GENMASK(6, 2))
 #define STPMIC1_LDO12356_OUTPUT_SHIFT	2
 #define STPMIC1_LDO3_MODE		(uint8_t)(BIT(7))
@@ -45,25 +37,29 @@
 	return fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
 }
 
-bool dt_check_pmic(void)
+int dt_pmic_status(void)
 {
 	int node;
 	void *fdt;
 
 	if (fdt_get_address(&fdt) == 0) {
-		return false;
+		return -ENOENT;
 	}
 
 	node = dt_get_pmic_node(fdt);
-	if (node < 0) {
-		VERBOSE("%s: No PMIC node found in DT\n", __func__);
-		return false;
+	if (node <= 0) {
+		return -FDT_ERR_NOTFOUND;
 	}
 
 	return fdt_get_status(node);
 }
 
-static int dt_pmic_i2c_config(struct dt_node_info *i2c_info)
+/*
+ * Get PMIC and its I2C bus configuration from the device tree.
+ * Return 0 on success, negative on error, 1 if no PMIC node is found.
+ */
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
+			      struct stm32_i2c_init_s *init)
 {
 	int pmic_node, i2c_node;
 	void *fdt;
@@ -75,7 +71,7 @@
 
 	pmic_node = dt_get_pmic_node(fdt);
 	if (pmic_node < 0) {
-		return -FDT_ERR_NOTFOUND;
+		return 1;
 	}
 
 	cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
@@ -98,10 +94,10 @@
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	return dt_set_pinctrl_config(i2c_node);
+	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
 }
 
-int dt_pmic_enable_boot_on_regulators(void)
+int dt_pmic_configure_boot_on_regulators(void)
 {
 	int pmic_node, regulators_node, regulator_node;
 	void *fdt;
@@ -119,14 +115,40 @@
 
 	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
 		const fdt32_t *cuint;
-		const char *node_name;
+		const char *node_name = fdt_get_name(fdt, regulator_node, NULL);
 		uint16_t voltage;
+		int status;
 
+#if defined(IMAGE_BL2)
+		if ((fdt_getprop(fdt, regulator_node, "regulator-boot-on",
+				 NULL) == NULL) &&
+		    (fdt_getprop(fdt, regulator_node, "regulator-always-on",
+				 NULL) == NULL)) {
+#else
 		if (fdt_getprop(fdt, regulator_node, "regulator-boot-on",
 				NULL) == NULL) {
+#endif
 			continue;
 		}
 
+		if (fdt_getprop(fdt, regulator_node, "regulator-pull-down",
+				NULL) != NULL) {
+
+			status = stpmic1_regulator_pull_down_set(node_name);
+			if (status != 0) {
+				return status;
+			}
+		}
+
+		if (fdt_getprop(fdt, regulator_node, "st,mask-reset",
+				NULL) != NULL) {
+
+			status = stpmic1_regulator_mask_reset_set(node_name);
+			if (status != 0) {
+				return status;
+			}
+		}
+
 		cuint = fdt_getprop(fdt, regulator_node,
 				    "regulator-min-microvolt", NULL);
 		if (cuint == NULL) {
@@ -135,17 +157,13 @@
 
 		/* DT uses microvolts, whereas driver awaits millivolts */
 		voltage = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
-		node_name = fdt_get_name(fdt, regulator_node, NULL);
 
-		if (stpmic1_is_regulator_enabled(node_name) == 0U) {
-			int status;
-
-			status = stpmic1_regulator_voltage_set(node_name,
-							       voltage);
-			if (status != 0) {
-				return status;
-			}
+		status = stpmic1_regulator_voltage_set(node_name, voltage);
+		if (status != 0) {
+			return status;
+		}
 
+		if (stpmic1_is_regulator_enabled(node_name) == 0U) {
 			status = stpmic1_regulator_enable(node_name);
 			if (status != 0) {
 				return status;
@@ -156,77 +174,77 @@
 	return 0;
 }
 
-void initialize_pmic_i2c(void)
+bool initialize_pmic_i2c(void)
 {
 	int ret;
 	struct dt_node_info i2c_info;
+	struct i2c_handle_s *i2c = &i2c_handle;
+	struct stm32_i2c_init_s i2c_init;
 
-	if (dt_pmic_i2c_config(&i2c_info) != 0) {
-		ERROR("I2C configuration failed\n");
+	ret = dt_pmic_i2c_config(&i2c_info, &i2c_init);
+	if (ret < 0) {
+		ERROR("I2C configuration failed %d\n", ret);
 		panic();
 	}
 
-	if (stm32mp_clk_enable((uint32_t)i2c_info.clock) < 0) {
-		ERROR("I2C clock enable failed\n");
-		panic();
+	if (ret != 0) {
+		return false;
 	}
 
 	/* Initialize PMIC I2C */
-	i2c_handle.i2c_base_addr		= i2c_info.base;
-	i2c_handle.i2c_init.timing		= I2C_TIMING;
-	i2c_handle.i2c_init.own_address1	= pmic_i2c_addr;
-	i2c_handle.i2c_init.addressing_mode	= I2C_ADDRESSINGMODE_7BIT;
-	i2c_handle.i2c_init.dual_address_mode	= I2C_DUALADDRESS_DISABLE;
-	i2c_handle.i2c_init.own_address2	= 0;
-	i2c_handle.i2c_init.own_address2_masks	= I2C_OAR2_OA2NOMASK;
-	i2c_handle.i2c_init.general_call_mode	= I2C_GENERALCALL_DISABLE;
-	i2c_handle.i2c_init.no_stretch_mode	= I2C_NOSTRETCH_DISABLE;
+	i2c->i2c_base_addr		= i2c_info.base;
+	i2c->dt_status			= i2c_info.status;
+	i2c->clock			= i2c_info.clock;
+	i2c_init.own_address1		= pmic_i2c_addr;
+	i2c_init.addressing_mode	= I2C_ADDRESSINGMODE_7BIT;
+	i2c_init.dual_address_mode	= I2C_DUALADDRESS_DISABLE;
+	i2c_init.own_address2		= 0;
+	i2c_init.own_address2_masks	= I2C_OAR2_OA2NOMASK;
+	i2c_init.general_call_mode	= I2C_GENERALCALL_DISABLE;
+	i2c_init.no_stretch_mode	= I2C_NOSTRETCH_DISABLE;
+	i2c_init.analog_filter		= 1;
+	i2c_init.digital_filter_coef	= 0;
 
-	ret = stm32_i2c_init(&i2c_handle);
+	ret = stm32_i2c_init(i2c, &i2c_init);
 	if (ret != 0) {
 		ERROR("Cannot initialize I2C %x (%d)\n",
-		      i2c_handle.i2c_base_addr, ret);
+		      i2c->i2c_base_addr, ret);
 		panic();
 	}
 
-	ret = stm32_i2c_config_analog_filter(&i2c_handle,
-					     I2C_ANALOGFILTER_ENABLE);
-	if (ret != 0) {
-		ERROR("Cannot initialize I2C analog filter (%d)\n", ret);
+	if (!stm32_i2c_is_device_ready(i2c, pmic_i2c_addr, 1,
+				       I2C_TIMEOUT_BUSY_MS)) {
+		ERROR("I2C device not ready\n");
 		panic();
 	}
 
-	ret = stm32_i2c_is_device_ready(&i2c_handle, (uint16_t)pmic_i2c_addr, 1,
-					I2C_TIMEOUT);
-	if (ret != 0) {
-		ERROR("I2C device not ready (%d)\n", ret);
-		panic();
-	}
+	stpmic1_bind_i2c(i2c, (uint16_t)pmic_i2c_addr);
 
-	stpmic1_bind_i2c(&i2c_handle, (uint16_t)pmic_i2c_addr);
+	return true;
 }
 
 void initialize_pmic(void)
 {
-	int status;
-	uint8_t read_val;
+	unsigned long pmic_version;
 
-	initialize_pmic_i2c();
+	if (!initialize_pmic_i2c()) {
+		VERBOSE("No PMIC\n");
+		return;
+	}
 
-	status = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
-	if (status != 0) {
+	if (stpmic1_get_version(&pmic_version) != 0) {
+		ERROR("Failed to access PMIC\n");
 		panic();
 	}
 
-	INFO("PMIC version = 0x%x\n", read_val);
+	INFO("PMIC version = 0x%02lx\n", pmic_version);
+	stpmic1_dump_regulators();
 
-	/* Keep VDD on during the reset cycle */
-	status = stpmic1_register_update(MASK_RESET_BUCK_REG,
-					MASK_RESET_BUCK3,
-					MASK_RESET_BUCK3);
-	if (status != 0) {
+#if defined(IMAGE_BL2)
+	if (dt_pmic_configure_boot_on_regulators() != 0) {
 		panic();
-	}
+	};
+#endif
 }
 
 int pmic_ddr_power_init(enum ddr_type ddr_type)
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
index 465996d..9999630 100644
--- a/drivers/st/pmic/stpmic1.c
+++ b/drivers/st/pmic/stpmic1.c
@@ -8,7 +8,8 @@
 
 #include <common/debug.h>
 #include <drivers/st/stpmic1.h>
-#include <plat/common/platform.h>
+
+#define I2C_TIMEOUT_MS		25
 
 struct regul_struct {
 	const char *dt_node_name;
@@ -677,8 +678,9 @@
 int stpmic1_register_read(uint8_t register_id,  uint8_t *value)
 {
 	return stm32_i2c_mem_read(pmic_i2c_handle, pmic_i2c_addr,
-				  (uint16_t)register_id, I2C_MEMADD_SIZE_8BIT,
-				  value, 1, 100000);
+				  (uint16_t)register_id,
+				  I2C_MEMADD_SIZE_8BIT, value,
+				  1, I2C_TIMEOUT_MS);
 }
 
 int stpmic1_register_write(uint8_t register_id, uint8_t value)
@@ -687,7 +689,8 @@
 
 	status = stm32_i2c_mem_write(pmic_i2c_handle, pmic_i2c_addr,
 				     (uint16_t)register_id,
-				     I2C_MEMADD_SIZE_8BIT, &value, 1, 100000);
+				     I2C_MEMADD_SIZE_8BIT, &value,
+				     1, I2C_TIMEOUT_MS);
 
 #if ENABLE_ASSERTIONS
 	if (status != 0) {
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index a97e805..5d8817f 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -55,7 +55,7 @@
 
 			vddcore: buck1 {
 				regulator-name = "vddcore";
-				regulator-min-microvolt = <800000>;
+				regulator-min-microvolt = <1200000>;
 				regulator-max-microvolt = <1350000>;
 				regulator-always-on;
 				regulator-initial-mode = <0>;
diff --git a/include/drivers/st/stm32_i2c.h b/include/drivers/st/stm32_i2c.h
index de2ca59..170d4cf 100644
--- a/include/drivers/st/stm32_i2c.h
+++ b/include/drivers/st/stm32_i2c.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -111,94 +111,113 @@
 #define I2C_ICR_TIMOUTCF		BIT(12)
 #define I2C_ICR_ALERTCF			BIT(13)
 
+enum i2c_speed_e {
+	I2C_SPEED_STANDARD,	/* 100 kHz */
+	I2C_SPEED_FAST,		/* 400 kHz */
+	I2C_SPEED_FAST_PLUS,	/* 1 MHz   */
+};
+
+#define STANDARD_RATE				100000
+#define FAST_RATE				400000
+#define FAST_PLUS_RATE				1000000
+
 struct stm32_i2c_init_s {
-	uint32_t timing;           /* Specifies the I2C_TIMINGR_register value
-				    * This parameter is calculated by referring
-				    * to I2C initialization section in Reference
-				    * manual.
-				    */
+	uint32_t own_address1;		/*
+					 * Specifies the first device own
+					 * address. This parameter can be a
+					 * 7-bit or 10-bit address.
+					 */
 
-	uint32_t own_address1;     /* Specifies the first device own address.
-				    * This parameter can be a 7-bit or 10-bit
-				    * address.
-				    */
+	uint32_t addressing_mode;	/*
+					 * Specifies if 7-bit or 10-bit
+					 * addressing mode is selected.
+					 * This parameter can be a value of
+					 * @ref I2C_ADDRESSING_MODE.
+					 */
 
-	uint32_t addressing_mode;  /* Specifies if 7-bit or 10-bit addressing
-				    * mode is selected.
-				    * This parameter can be a value of @ref
-				    * I2C_ADDRESSING_MODE.
-				    */
+	uint32_t dual_address_mode;	/*
+					 * Specifies if dual addressing mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_DUAL_ADDRESSING_MODE.
+					 */
 
-	uint32_t dual_address_mode; /* Specifies if dual addressing mode is
-				     * selected.
-				     * This parameter can be a value of @ref
-				     * I2C_DUAL_ADDRESSING_MODE.
-				     */
+	uint32_t own_address2;		/*
+					 * Specifies the second device own
+					 * address if dual addressing mode is
+					 * selected. This parameter can be a
+					 * 7-bit address.
+					 */
 
-	uint32_t own_address2;     /* Specifies the second device own address
-				    * if dual addressing mode is selected.
-				    * This parameter can be a 7-bit address.
-				    */
+	uint32_t own_address2_masks;	/*
+					 * Specifies the acknowledge mask
+					 * address second device own address
+					 * if dual addressing mode is selected
+					 * This parameter can be a value of @ref
+					 * I2C_OWN_ADDRESS2_MASKS.
+					 */
 
-	uint32_t own_address2_masks; /* Specifies the acknowledge mask address
-				      * second device own address if dual
-				      * addressing mode is selected.
-				      * This parameter can be a value of @ref
-				      * I2C_OWN_ADDRESS2_MASKS.
-				      */
+	uint32_t general_call_mode;	/*
+					 * Specifies if general call mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_GENERAL_CALL_ADDRESSING_MODE.
+					 */
 
-	uint32_t general_call_mode; /* Specifies if general call mode is
-				     * selected.
-				     * This parameter can be a value of @ref
-				     * I2C_GENERAL_CALL_ADDRESSING_MODE.
-				     */
+	uint32_t no_stretch_mode;	/*
+					 * Specifies if nostretch mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_NOSTRETCH_MODE.
+					 */
 
-	uint32_t no_stretch_mode;  /* Specifies if nostretch mode is
-				    * selected.
-				    * This parameter can be a value of @ref
-				    * I2C_NOSTRETCH_MODE.
-				    */
+	uint32_t rise_time;		/*
+					 * Specifies the SCL clock pin rising
+					 * time in nanoseconds.
+					 */
 
+	uint32_t fall_time;		/*
+					 * Specifies the SCL clock pin falling
+					 * time in nanoseconds.
+					 */
+
+	enum i2c_speed_e speed_mode;	/*
+					 * Specifies the I2C clock source
+					 * frequency mode.
+					 * This parameter can be a value of @ref
+					 * i2c_speed_mode_e.
+					 */
+
+	int analog_filter;		/*
+					 * Specifies if the I2C analog noise
+					 * filter is selected.
+					 * This parameter can be 0 (filter
+					 * off), all other values mean filter
+					 * on.
+					 */
+
+	uint8_t digital_filter_coef;	/*
+					 * Specifies the I2C digital noise
+					 * filter coefficient.
+					 * This parameter can be a value
+					 * between 0 and
+					 * STM32_I2C_DIGITAL_FILTER_MAX.
+					 */
 };
 
 enum i2c_state_e {
-	I2C_STATE_RESET          = 0x00U,   /* Peripheral is not yet
-					     * initialized.
-					     */
-	I2C_STATE_READY          = 0x20U,   /* Peripheral Initialized
-					     * and ready for use.
-					     */
-	I2C_STATE_BUSY           = 0x24U,   /* An internal process is
-					     * ongoing.
-					     */
-	I2C_STATE_BUSY_TX        = 0x21U,   /* Data Transmission process
-					     * is ongoing.
-					     */
-	I2C_STATE_BUSY_RX        = 0x22U,   /* Data Reception process
-					     * is ongoing.
-					     */
-	I2C_STATE_LISTEN         = 0x28U,   /* Address Listen Mode is
-					     * ongoing.
-					     */
-	I2C_STATE_BUSY_TX_LISTEN = 0x29U,   /* Address Listen Mode
-					     * and Data Transmission
-					     * process is ongoing.
-					     */
-	I2C_STATE_BUSY_RX_LISTEN = 0x2AU,   /* Address Listen Mode
-					     * and Data Reception
-					     * process is ongoing.
-					     */
-	I2C_STATE_ABORT          = 0x60U,   /* Abort user request ongoing. */
-	I2C_STATE_TIMEOUT        = 0xA0U,   /* Timeout state. */
-	I2C_STATE_ERROR          = 0xE0U    /* Error. */
-
+	I2C_STATE_RESET          = 0x00U,	/* Not yet initialized       */
+	I2C_STATE_READY          = 0x20U,	/* Ready for use             */
+	I2C_STATE_BUSY           = 0x24U,	/* Internal process ongoing  */
+	I2C_STATE_BUSY_TX        = 0x21U,	/* Data Transmission ongoing */
+	I2C_STATE_BUSY_RX        = 0x22U,	/* Data Reception ongoing    */
 };
 
 enum i2c_mode_e {
-	I2C_MODE_NONE   = 0x00U,   /* No I2C communication on going.       */
-	I2C_MODE_MASTER = 0x10U,   /* I2C communication is in Master Mode. */
-	I2C_MODE_SLAVE  = 0x20U,   /* I2C communication is in Slave Mode.  */
-	I2C_MODE_MEM    = 0x40U    /* I2C communication is in Memory Mode. */
+	I2C_MODE_NONE   = 0x00U,	/* No active communication      */
+	I2C_MODE_MASTER = 0x10U,	/* Communication in Master Mode */
+	I2C_MODE_SLAVE  = 0x20U,	/* Communication in Slave Mode  */
+	I2C_MODE_MEM    = 0x40U		/* Communication in Memory Mode */
 
 };
 
@@ -213,26 +232,12 @@
 
 struct i2c_handle_s {
 	uint32_t i2c_base_addr;			/* Registers base address */
-
-	struct stm32_i2c_init_s i2c_init;	/* Communication parameters */
-
-	uint8_t *p_buff;			/* Pointer to transfer buffer */
-
-	uint16_t xfer_size;			/* Transfer size */
-
-	uint16_t xfer_count;			/* Transfer counter */
-
-	uint32_t prev_state;			/* Communication previous
-						 * state
-						 */
-
-	uint8_t lock;				/* Locking object */
-
-	enum i2c_state_e i2c_state;		/* Communication state */
-
-	enum i2c_mode_e i2c_mode;		/* Communication mode */
-
-	uint32_t i2c_err;			/* Error code */
+	unsigned int dt_status;			/* DT nsec/sec status     */
+	unsigned int clock;			/* Clock reference        */
+	uint8_t lock;				/* Locking object         */
+	enum i2c_state_e i2c_state;		/* Communication state    */
+	enum i2c_mode_e i2c_mode;		/* Communication mode     */
+	uint32_t i2c_err;			/* Error code             */
 };
 
 #define I2C_ADDRESSINGMODE_7BIT		0x00000001U
@@ -250,15 +255,15 @@
 #define I2C_MEMADD_SIZE_8BIT		0x00000001U
 #define I2C_MEMADD_SIZE_16BIT		0x00000002U
 
-#define  I2C_RELOAD_MODE		I2C_CR2_RELOAD
-#define  I2C_AUTOEND_MODE		I2C_CR2_AUTOEND
-#define  I2C_SOFTEND_MODE		0x00000000U
+#define I2C_RELOAD_MODE			I2C_CR2_RELOAD
+#define I2C_AUTOEND_MODE		I2C_CR2_AUTOEND
+#define I2C_SOFTEND_MODE		0x00000000U
 
-#define  I2C_NO_STARTSTOP		0x00000000U
-#define  I2C_GENERATE_STOP		(BIT(31) | I2C_CR2_STOP)
-#define  I2C_GENERATE_START_READ	(BIT(31) | I2C_CR2_START | \
+#define I2C_NO_STARTSTOP		0x00000000U
+#define I2C_GENERATE_STOP		(BIT(31) | I2C_CR2_STOP)
+#define I2C_GENERATE_START_READ		(BIT(31) | I2C_CR2_START | \
 					 I2C_CR2_RD_WRN)
-#define  I2C_GENERATE_START_WRITE	(BIT(31) | I2C_CR2_START)
+#define I2C_GENERATE_START_WRITE	(BIT(31) | I2C_CR2_START)
 
 #define I2C_FLAG_TXE			I2C_ISR_TXE
 #define I2C_FLAG_TXIS			I2C_ISR_TXIS
@@ -281,21 +286,36 @@
 					 I2C_CR2_NBYTES | I2C_CR2_RELOAD  | \
 					 I2C_CR2_RD_WRN)
 
-#define I2C_ANALOGFILTER_ENABLE		((uint32_t)0x00000000U)
+#define I2C_TIMEOUT_BUSY_MS		25U
+
+#define I2C_ANALOGFILTER_ENABLE		0x00000000U
 #define I2C_ANALOGFILTER_DISABLE	I2C_CR1_ANFOFF
 
-int stm32_i2c_init(struct i2c_handle_s *hi2c);
+/* STM32 specific defines */
+#define STM32_I2C_RISE_TIME_DEFAULT		25	/* ns */
+#define STM32_I2C_FALL_TIME_DEFAULT		10	/* ns */
+#define STM32_I2C_SPEED_DEFAULT			I2C_SPEED_STANDARD
+#define STM32_I2C_ANALOG_FILTER_DELAY_MIN	50	/* ns */
+#define STM32_I2C_ANALOG_FILTER_DELAY_MAX	260	/* ns */
+#define STM32_I2C_DIGITAL_FILTER_MAX		16
 
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+				 struct stm32_i2c_init_s *init);
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+		   struct stm32_i2c_init_s *init_data);
 int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 			uint16_t mem_addr, uint16_t mem_add_size,
-			uint8_t *p_data, uint16_t size, uint32_t timeout);
+			uint8_t *p_data, uint16_t size, uint32_t timeout_ms);
 int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 		       uint16_t mem_addr, uint16_t mem_add_size,
-		       uint8_t *p_data, uint16_t size, uint32_t timeout);
-int stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c, uint16_t dev_addr,
-			      uint32_t trials, uint32_t timeout);
-
-int stm32_i2c_config_analog_filter(struct i2c_handle_s *hi2c,
-				   uint32_t analog_filter);
+		       uint8_t *p_data, uint16_t size, uint32_t timeout_ms);
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      uint8_t *p_data, uint16_t size,
+			      uint32_t timeout_ms);
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			     uint8_t *p_data, uint16_t size,
+			     uint32_t timeout_ms);
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			       uint32_t trials, uint32_t timeout_ms);
 
 #endif /* STM32_I2C_H */
diff --git a/include/drivers/st/stm32mp_pmic.h b/include/drivers/st/stm32mp_pmic.h
index 700039b..984cd60 100644
--- a/include/drivers/st/stm32mp_pmic.h
+++ b/include/drivers/st/stm32mp_pmic.h
@@ -11,10 +11,41 @@
 
 #include <platform_def.h>
 
-bool dt_check_pmic(void);
-int dt_pmic_enable_boot_on_regulators(void);
-void initialize_pmic_i2c(void);
+/*
+ * dt_pmic_status - Check PMIC status from device tree
+ *
+ * Returns the status of the PMIC (secure, non-secure), or a negative value on
+ * error
+ */
+int dt_pmic_status(void);
+
+/*
+ * dt_pmic_configure_boot_on_regulators - Configure boot-on and always-on
+ * regulators from device tree configuration
+ *
+ * Returns 0 on success, and negative values on errors
+ */
+int dt_pmic_configure_boot_on_regulators(void);
+
+/*
+ * initialize_pmic_i2c - Initialize I2C for the PMIC control
+ *
+ * Returns true if PMIC is available, false if not found, panics on errors
+ */
+bool initialize_pmic_i2c(void);
+
+/*
+ * initialize_pmic - Main PMIC initialization function, called at platform init
+ *
+ * Panics on errors
+ */
 void initialize_pmic(void);
+
+/*
+ * pmic_ddr_power_init - Initialize regulators required for DDR
+ *
+ * Returns 0 on success, and negative values on errors
+ */
 int pmic_ddr_power_init(enum ddr_type ddr_type);
 
 #endif /* STM32MP_PMIC_H */
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index bcf1aa9..69dc3fb 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -126,7 +126,7 @@
 {
 	int ret;
 
-	if (dt_check_pmic()) {
+	if (dt_pmic_status() > 0) {
 		initialize_pmic();
 	}