Merge tag 'v2022.04-rc5' into next
[platform/kernel/u-boot.git] / drivers / i2c / stm32f7_i2c.c
index 5ca0b7d..c6ae65b 100644 (file)
@@ -1,16 +1,23 @@
+// SPDX-License-Identifier: GPL-2.0+
 /*
  * (C) Copyright 2017 STMicroelectronics
- *
- * SPDX-License-Identifier:    GPL-2.0+
  */
 
+#define LOG_CATEGORY UCLASS_I2C
+
 #include <common.h>
 #include <clk.h>
 #include <dm.h>
 #include <i2c.h>
+#include <log.h>
+#include <regmap.h>
 #include <reset.h>
-
+#include <syscon.h>
 #include <dm/device.h>
+#include <dm/device_compat.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/io.h>
 
 /* STM32 I2C registers */
@@ -38,6 +45,8 @@ struct stm32_i2c_regs {
 
 /* STM32 I2C control 1 */
 #define STM32_I2C_CR1_ANFOFF                   BIT(12)
+#define STM32_I2C_CR1_DNF_MASK                 GENMASK(11, 8)
+#define STM32_I2C_CR1_DNF(n)                   (((n) & 0xf) << 8)
 #define STM32_I2C_CR1_ERRIE                    BIT(7)
 #define STM32_I2C_CR1_TCIE                     BIT(6)
 #define STM32_I2C_CR1_STOPIE                   BIT(5)
@@ -59,7 +68,7 @@ struct stm32_i2c_regs {
 #define STM32_I2C_CR2_ADD10                    BIT(11)
 #define STM32_I2C_CR2_RD_WRN                   BIT(10)
 #define STM32_I2C_CR2_SADD10_MASK              GENMASK(9, 0)
-#define STM32_I2C_CR2_SADD10(n)                        ((n & STM32_I2C_CR2_SADD10_MASK))
+#define STM32_I2C_CR2_SADD10(n)                        (n & STM32_I2C_CR2_SADD10_MASK)
 #define STM32_I2C_CR2_SADD7_MASK               GENMASK(7, 1)
 #define STM32_I2C_CR2_SADD7(n)                 ((n & 0x7f) << 1)
 #define STM32_I2C_CR2_RESET_MASK               (STM32_I2C_CR2_HEAD10R \
@@ -98,10 +107,8 @@ struct stm32_i2c_regs {
 
 #define STM32_I2C_MAX_LEN                      0xff
 
-#define STM32_I2C_DNF_DEFAULT                  0
-#define STM32_I2C_DNF_MAX                      16
+#define STM32_I2C_DNF_MAX                      15
 
-#define STM32_I2C_ANALOG_FILTER_ENABLE 1
 #define STM32_I2C_ANALOG_FILTER_DELAY_MIN      50      /* ns */
 #define STM32_I2C_ANALOG_FILTER_DELAY_MAX      260     /* ns */
 
@@ -116,17 +123,6 @@ struct stm32_i2c_regs {
 
 #define STM32_NSEC_PER_SEC                     1000000000L
 
-#define STANDARD_RATE                          100000
-#define FAST_RATE                              400000
-#define FAST_PLUS_RATE                         1000000
-
-enum stm32_i2c_speed {
-       STM32_I2C_SPEED_STANDARD, /* 100 kHz */
-       STM32_I2C_SPEED_FAST, /* 400 kHz */
-       STM32_I2C_SPEED_FAST_PLUS, /* 1 MHz */
-       STM32_I2C_SPEED_END,
-};
-
 /**
  * struct stm32_i2c_spec - private i2c specification timing
  * @rate: I2C bus speed (Hz)
@@ -156,16 +152,14 @@ struct stm32_i2c_spec {
 
 /**
  * struct stm32_i2c_setup - private I2C timing setup parameters
- * @speed: I2C speed mode (standard, Fast Plus)
  * @speed_freq: I2C speed frequency  (Hz)
  * @clock_src: I2C clock source frequency (Hz)
  * @rise_time: Rise time (ns)
  * @fall_time: Fall time (ns)
- * @dnf: Digital filter coefficient (0-16)
+ * @dnf: value of digital filter to apply
  * @analog_filter: Analog filter delay (On/Off)
  */
 struct stm32_i2c_setup {
-       enum stm32_i2c_speed speed;
        u32 speed_freq;
        u32 clock_src;
        u32 rise_time;
@@ -175,6 +169,14 @@ struct stm32_i2c_setup {
 };
 
 /**
+ * struct stm32_i2c_data - driver data for I2C configuration by compatible
+ * @fmp_clr_offset: Fast Mode Plus clear register offset from set register
+ */
+struct stm32_i2c_data {
+       u32 fmp_clr_offset;
+};
+
+/**
  * struct stm32_i2c_timings - private I2C output parameters
  * @prec: Prescaler value
  * @scldel: Data setup time
@@ -191,16 +193,34 @@ struct stm32_i2c_timings {
        u8 scll;
 };
 
+/**
+ * struct stm32_i2c_priv - private data of the controller
+ * @regs: I2C registers address
+ * @clk: hw i2c clock
+ * @setup: I2C timing setup parameters
+ * @speed: I2C clock frequency of the controller. Standard, Fast or Fast+
+ * @regmap: holds SYSCFG phandle for Fast Mode Plus bit
+ * @regmap_sreg: register address for setting Fast Mode Plus bits
+ * @regmap_creg: register address for clearing Fast Mode Plus bits
+ * @regmap_mask: mask for Fast Mode Plus bits
+ * @dnf_dt: value of digital filter requested via dt
+ */
 struct stm32_i2c_priv {
        struct stm32_i2c_regs *regs;
        struct clk clk;
-       struct stm32_i2c_setup *setup;
-       int speed;
+       struct stm32_i2c_setup setup;
+       u32 speed;
+       struct regmap *regmap;
+       u32 regmap_sreg;
+       u32 regmap_creg;
+       u32 regmap_mask;
+       u32 dnf_dt;
 };
 
-static struct stm32_i2c_spec i2c_specs[] = {
-       [STM32_I2C_SPEED_STANDARD] = {
-               .rate = STANDARD_RATE,
+static const struct stm32_i2c_spec i2c_specs[] = {
+       /* Standard speed - 100 KHz */
+       [IC_SPEED_MODE_STANDARD] = {
+               .rate = I2C_SPEED_STANDARD_RATE,
                .rate_min = 8000,
                .rate_max = 120000,
                .fall_max = 300,
@@ -211,8 +231,9 @@ static struct stm32_i2c_spec i2c_specs[] = {
                .l_min = 4700,
                .h_min = 4000,
        },
-       [STM32_I2C_SPEED_FAST] = {
-               .rate = FAST_RATE,
+       /* Fast speed - 400 KHz */
+       [IC_SPEED_MODE_FAST] = {
+               .rate = I2C_SPEED_FAST_RATE,
                .rate_min = 320000,
                .rate_max = 480000,
                .fall_max = 300,
@@ -223,8 +244,9 @@ static struct stm32_i2c_spec i2c_specs[] = {
                .l_min = 1300,
                .h_min = 600,
        },
-       [STM32_I2C_SPEED_FAST_PLUS] = {
-               .rate = FAST_PLUS_RATE,
+       /* Fast Plus Speed - 1 MHz */
+       [IC_SPEED_MODE_FAST_PLUS] = {
+               .rate = I2C_SPEED_FAST_PLUS_RATE,
                .rate_min = 800000,
                .rate_max = 1200000,
                .fall_max = 100,
@@ -237,11 +259,12 @@ static struct stm32_i2c_spec i2c_specs[] = {
        },
 };
 
-static struct stm32_i2c_setup stm32f7_setup = {
-       .rise_time = STM32_I2C_RISE_TIME_DEFAULT,
-       .fall_time = STM32_I2C_FALL_TIME_DEFAULT,
-       .dnf = STM32_I2C_DNF_DEFAULT,
-       .analog_filter = STM32_I2C_ANALOG_FILTER_ENABLE,
+static const struct stm32_i2c_data stm32f7_data = {
+       .fmp_clr_offset = 0x00,
+};
+
+static const struct stm32_i2c_data stm32mp15_data = {
+       .fmp_clr_offset = 0x40,
 };
 
 static int stm32_i2c_check_device_busy(struct stm32_i2c_priv *i2c_priv)
@@ -256,7 +279,7 @@ static int stm32_i2c_check_device_busy(struct stm32_i2c_priv *i2c_priv)
 }
 
 static void stm32_i2c_message_start(struct stm32_i2c_priv *i2c_priv,
-                                     struct i2c_msg *msg, bool stop)
+                                   struct i2c_msg *msg, bool stop)
 {
        struct stm32_i2c_regs *regs = i2c_priv->regs;
        u32 cr2 = readl(&regs->cr2);
@@ -300,7 +323,7 @@ static void stm32_i2c_message_start(struct stm32_i2c_priv *i2c_priv,
  */
 
 static void stm32_i2c_handle_reload(struct stm32_i2c_priv *i2c_priv,
-                                     struct i2c_msg *msg, bool stop)
+                                   struct i2c_msg *msg, bool stop)
 {
        struct stm32_i2c_regs *regs = i2c_priv->regs;
        u32 cr2 = readl(&regs->cr2);
@@ -318,7 +341,7 @@ static void stm32_i2c_handle_reload(struct stm32_i2c_priv *i2c_priv,
 }
 
 static int stm32_i2c_wait_flags(struct stm32_i2c_priv *i2c_priv,
-                                 u32 flags, u32 *status)
+                               u32 flags, u32 *status)
 {
        struct stm32_i2c_regs *regs = i2c_priv->regs;
        u32 time_start = get_timer(0);
@@ -326,7 +349,7 @@ static int stm32_i2c_wait_flags(struct stm32_i2c_priv *i2c_priv,
        *status = readl(&regs->isr);
        while (!(*status & flags)) {
                if (get_timer(time_start) > CONFIG_SYS_HZ) {
-                       debug("%s: i2c timeout\n", __func__);
+                       log_debug("i2c timeout\n");
                        return -ETIMEDOUT;
                }
 
@@ -349,7 +372,7 @@ static int stm32_i2c_check_end_of_message(struct stm32_i2c_priv *i2c_priv)
                return ret;
 
        if (status & STM32_I2C_ISR_BERR) {
-               debug("%s: Bus error\n", __func__);
+               log_debug("Bus error\n");
 
                /* Clear BERR flag */
                setbits_le32(&regs->icr, STM32_I2C_ICR_BERRCF);
@@ -358,7 +381,7 @@ static int stm32_i2c_check_end_of_message(struct stm32_i2c_priv *i2c_priv)
        }
 
        if (status & STM32_I2C_ISR_ARLO) {
-               debug("%s: Arbitration lost\n", __func__);
+               log_debug("Arbitration lost\n");
 
                /* Clear ARLO flag */
                setbits_le32(&regs->icr, STM32_I2C_ICR_ARLOCF);
@@ -367,7 +390,7 @@ static int stm32_i2c_check_end_of_message(struct stm32_i2c_priv *i2c_priv)
        }
 
        if (status & STM32_I2C_ISR_NACKF) {
-               debug("%s: Receive NACK\n", __func__);
+               log_debug("Receive NACK\n");
 
                /* Clear NACK flag */
                setbits_le32(&regs->icr, STM32_I2C_ICR_NACKCF);
@@ -393,7 +416,7 @@ static int stm32_i2c_check_end_of_message(struct stm32_i2c_priv *i2c_priv)
 }
 
 static int stm32_i2c_message_xfer(struct stm32_i2c_priv *i2c_priv,
-                                   struct i2c_msg *msg, bool stop)
+                                 struct i2c_msg *msg, bool stop)
 {
        struct stm32_i2c_regs *regs = i2c_priv->regs;
        u32 status;
@@ -466,7 +489,7 @@ static int stm32_i2c_message_xfer(struct stm32_i2c_priv *i2c_priv,
 }
 
 static int stm32_i2c_xfer(struct udevice *bus, struct i2c_msg *msg,
-                           int nmsgs)
+                         int nmsgs)
 {
        struct stm32_i2c_priv *i2c_priv = dev_get_priv(bus);
        int ret;
@@ -484,13 +507,13 @@ static int stm32_i2c_xfer(struct udevice *bus, struct i2c_msg *msg,
        return 0;
 }
 
-static int stm32_i2c_compute_solutions(struct stm32_i2c_setup *setup,
+static int stm32_i2c_compute_solutions(u32 i2cclk,
+                                      struct stm32_i2c_setup *setup,
+                                      const struct stm32_i2c_spec *specs,
                                       struct list_head *solutions)
 {
        struct stm32_i2c_timings *v;
        u32 p_prev = STM32_PRESC_MAX;
-       u32 i2cclk = DIV_ROUND_CLOSEST(STM32_NSEC_PER_SEC,
-                                      setup->clock_src);
        u32 af_delay_min, af_delay_max;
        u16 p, l, a;
        int sdadel_min, sdadel_max, scldel_min;
@@ -501,32 +524,32 @@ static int stm32_i2c_compute_solutions(struct stm32_i2c_setup *setup,
        af_delay_max = setup->analog_filter ?
                       STM32_I2C_ANALOG_FILTER_DELAY_MAX : 0;
 
-       sdadel_min = setup->fall_time - i2c_specs[setup->speed].hddat_min -
+       sdadel_min = specs->hddat_min + setup->fall_time -
                     af_delay_min - (setup->dnf + 3) * i2cclk;
 
-       sdadel_max = i2c_specs[setup->speed].vddat_max - setup->rise_time -
+       sdadel_max = specs->vddat_max - setup->rise_time -
                     af_delay_max - (setup->dnf + 4) * i2cclk;
 
-       scldel_min = setup->rise_time + i2c_specs[setup->speed].sudat_min;
+       scldel_min = setup->rise_time + specs->sudat_min;
 
        if (sdadel_min < 0)
                sdadel_min = 0;
        if (sdadel_max < 0)
                sdadel_max = 0;
 
-       debug("%s: SDADEL(min/max): %i/%i, SCLDEL(Min): %i\n", __func__,
-             sdadel_min, sdadel_max, scldel_min);
+       log_debug("SDADEL(min/max): %i/%i, SCLDEL(Min): %i\n",
+                 sdadel_min, sdadel_max, scldel_min);
 
        /* Compute possible values for PRESC, SCLDEL and SDADEL */
        for (p = 0; p < STM32_PRESC_MAX; p++) {
                for (l = 0; l < STM32_SCLDEL_MAX; l++) {
-                       u32 scldel = (l + 1) * (p + 1) * i2cclk;
+                       int scldel = (l + 1) * (p + 1) * i2cclk;
 
                        if (scldel < scldel_min)
                                continue;
 
                        for (a = 0; a < STM32_SDADEL_MAX; a++) {
-                               u32 sdadel = (a * (p + 1) + 1) * i2cclk;
+                               int sdadel = (a * (p + 1) + 1) * i2cclk;
 
                                if (((sdadel >= sdadel_min) &&
                                     (sdadel <= sdadel_max)) &&
@@ -541,20 +564,26 @@ static int stm32_i2c_compute_solutions(struct stm32_i2c_setup *setup,
                                        p_prev = p;
 
                                        list_add_tail(&v->node, solutions);
+                                       break;
                                }
                        }
+
+                       if (p_prev == p)
+                               break;
                }
        }
 
        if (list_empty(solutions)) {
-               pr_err("%s: no Prescaler solution\n", __func__);
+               log_err("no Prescaler solution\n");
                ret = -EPERM;
        }
 
        return ret;
 }
 
-static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
+static int stm32_i2c_choose_solution(u32 i2cclk,
+                                    struct stm32_i2c_setup *setup,
+                                    const struct stm32_i2c_spec *specs,
                                     struct list_head *solutions,
                                     struct stm32_i2c_timings *s)
 {
@@ -562,8 +591,6 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
        u32 i2cbus = DIV_ROUND_CLOSEST(STM32_NSEC_PER_SEC,
                                       setup->speed_freq);
        u32 clk_error_prev = i2cbus;
-       u32 i2cclk = DIV_ROUND_CLOSEST(STM32_NSEC_PER_SEC,
-                                      setup->clock_src);
        u32 clk_min, clk_max;
        u32 af_delay_min;
        u32 dnf_delay;
@@ -577,8 +604,8 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
        dnf_delay = setup->dnf * i2cclk;
 
        tsync = af_delay_min + dnf_delay + (2 * i2cclk);
-       clk_max = STM32_NSEC_PER_SEC / i2c_specs[setup->speed].rate_min;
-       clk_min = STM32_NSEC_PER_SEC / i2c_specs[setup->speed].rate_max;
+       clk_max = STM32_NSEC_PER_SEC / specs->rate_min;
+       clk_min = STM32_NSEC_PER_SEC / specs->rate_max;
 
        /*
         * Among Prescaler possibilities discovered above figures out SCL Low
@@ -595,7 +622,8 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
 
                for (l = 0; l < STM32_SCLL_MAX; l++) {
                        u32 tscl_l = (l + 1) * prescaler + tsync;
-                       if ((tscl_l < i2c_specs[setup->speed].l_min) ||
+
+                       if (tscl_l < specs->l_min ||
                            (i2cclk >=
                             ((tscl_l - af_delay_min - dnf_delay) / 4))) {
                                continue;
@@ -607,12 +635,14 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
                                           setup->rise_time + setup->fall_time;
 
                                if ((tscl >= clk_min) && (tscl <= clk_max) &&
-                                   (tscl_h >= i2c_specs[setup->speed].h_min) &&
+                                   (tscl_h >= specs->h_min) &&
                                    (i2cclk < tscl_h)) {
-                                       int clk_error = tscl - i2cbus;
+                                       u32 clk_error;
 
-                                       if (clk_error < 0)
-                                               clk_error = -clk_error;
+                                       if (tscl > i2cbus)
+                                               clk_error = tscl - i2cbus;
+                                       else
+                                               clk_error = i2cbus - tscl;
 
                                        if (clk_error < clk_error_prev) {
                                                clk_error_prev = clk_error;
@@ -627,61 +657,71 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
        }
 
        if (!sol_found) {
-               pr_err("%s: no solution at all\n", __func__);
+               log_err("no solution at all\n");
                ret = -EPERM;
        }
 
        return ret;
 }
 
+static const struct stm32_i2c_spec *get_specs(u32 rate)
+{
+       unsigned int i;
+
+       for (i = 0; i < ARRAY_SIZE(i2c_specs); i++)
+               if (rate <= i2c_specs[i].rate)
+                       return &i2c_specs[i];
+
+       /* NOT REACHED */
+       return ERR_PTR(-EINVAL);
+}
+
 static int stm32_i2c_compute_timing(struct stm32_i2c_priv *i2c_priv,
-                                     struct stm32_i2c_setup *setup,
-                                     struct stm32_i2c_timings *output)
+                                   struct stm32_i2c_timings *output)
 {
+       struct stm32_i2c_setup *setup = &i2c_priv->setup;
+       const struct stm32_i2c_spec *specs;
        struct stm32_i2c_timings *v, *_v;
        struct list_head solutions;
+       u32 i2cclk = DIV_ROUND_CLOSEST(STM32_NSEC_PER_SEC, setup->clock_src);
        int ret;
 
-       if (setup->speed >= STM32_I2C_SPEED_END) {
-               pr_err("%s: speed out of bound {%d/%d}\n", __func__,
-                     setup->speed, STM32_I2C_SPEED_END - 1);
+       specs = get_specs(setup->speed_freq);
+       if (specs == ERR_PTR(-EINVAL)) {
+               log_err("speed out of bound {%d}\n",
+                       setup->speed_freq);
                return -EINVAL;
        }
 
-       if ((setup->rise_time > i2c_specs[setup->speed].rise_max) ||
-           (setup->fall_time > i2c_specs[setup->speed].fall_max)) {
-               pr_err("%s :timings out of bound Rise{%d>%d}/Fall{%d>%d}\n",
-                     __func__,
-                     setup->rise_time, i2c_specs[setup->speed].rise_max,
-                     setup->fall_time, i2c_specs[setup->speed].fall_max);
+       if (setup->rise_time > specs->rise_max ||
+           setup->fall_time > specs->fall_max) {
+               log_err("timings out of bound Rise{%d>%d}/Fall{%d>%d}\n",
+                       setup->rise_time, specs->rise_max,
+                       setup->fall_time, specs->fall_max);
                return -EINVAL;
        }
 
+       /*  Analog and Digital Filters */
+       setup->dnf = DIV_ROUND_CLOSEST(i2c_priv->dnf_dt, i2cclk);
        if (setup->dnf > STM32_I2C_DNF_MAX) {
-               pr_err("%s: DNF out of bound %d/%d\n", __func__,
-                     setup->dnf, STM32_I2C_DNF_MAX);
-               return -EINVAL;
-       }
-
-       if (setup->speed_freq > i2c_specs[setup->speed].rate) {
-               pr_err("%s: Freq {%d/%d}\n", __func__,
-                     setup->speed_freq, i2c_specs[setup->speed].rate);
+               log_err("DNF out of bound %d/%d\n",
+                       setup->dnf, STM32_I2C_DNF_MAX);
                return -EINVAL;
        }
 
        INIT_LIST_HEAD(&solutions);
-       ret = stm32_i2c_compute_solutions(setup, &solutions);
+       ret = stm32_i2c_compute_solutions(i2cclk, setup, specs, &solutions);
        if (ret)
                goto exit;
 
-       ret = stm32_i2c_choose_solution(setup, &solutions, output);
+       ret = stm32_i2c_choose_solution(i2cclk, setup, specs, &solutions, output);
        if (ret)
                goto exit;
 
-       debug("%s: Presc: %i, scldel: %i, sdadel: %i, scll: %i, sclh: %i\n",
-             __func__, output->presc,
-             output->scldel, output->sdadel,
-             output->scll, output->sclh);
+       log_debug("Presc: %i, scldel: %i, sdadel: %i, scll: %i, sclh: %i\n",
+                 output->presc,
+                 output->scldel, output->sdadel,
+                 output->scll, output->sclh);
 
 exit:
        /* Release list and memory */
@@ -693,33 +733,40 @@ exit:
        return ret;
 }
 
+static u32 get_lower_rate(u32 rate)
+{
+       int i;
+
+       for (i = ARRAY_SIZE(i2c_specs) - 1; i >= 0; i--)
+               if (rate > i2c_specs[i].rate)
+                       return i2c_specs[i].rate;
+
+       return i2c_specs[0].rate;
+}
+
 static int stm32_i2c_setup_timing(struct stm32_i2c_priv *i2c_priv,
-                                   struct stm32_i2c_timings *timing)
+                                 struct stm32_i2c_timings *timing)
 {
-       struct stm32_i2c_setup *setup = i2c_priv->setup;
+       struct stm32_i2c_setup *setup = &i2c_priv->setup;
        int ret = 0;
 
-       setup->speed = i2c_priv->speed;
-       setup->speed_freq = i2c_specs[setup->speed].rate;
+       setup->speed_freq = i2c_priv->speed;
        setup->clock_src = clk_get_rate(&i2c_priv->clk);
 
        if (!setup->clock_src) {
-               pr_err("%s: clock rate is 0\n", __func__);
+               log_err("clock rate is 0\n");
                return -EINVAL;
        }
 
        do {
-               ret = stm32_i2c_compute_timing(i2c_priv, setup, timing);
+               ret = stm32_i2c_compute_timing(i2c_priv, timing);
                if (ret) {
-                       debug("%s: failed to compute I2C timings.\n",
-                             __func__);
-                       if (i2c_priv->speed > STM32_I2C_SPEED_STANDARD) {
-                               i2c_priv->speed--;
-                               setup->speed = i2c_priv->speed;
+                       log_debug("failed to compute I2C timings.\n");
+                       if (setup->speed_freq > I2C_SPEED_STANDARD_RATE) {
                                setup->speed_freq =
-                                       i2c_specs[setup->speed].rate;
-                               debug("%s: downgrade I2C Speed Freq to (%i)\n",
-                                     __func__, i2c_specs[setup->speed].rate);
+                                       get_lower_rate(setup->speed_freq);
+                               log_debug("downgrade I2C Speed Freq to (%i)\n",
+                                         setup->speed_freq);
                        } else {
                                break;
                        }
@@ -727,20 +774,45 @@ static int stm32_i2c_setup_timing(struct stm32_i2c_priv *i2c_priv,
        } while (ret);
 
        if (ret) {
-               pr_err("%s: impossible to compute I2C timings.\n", __func__);
+               log_err("impossible to compute I2C timings.\n");
                return ret;
        }
 
-       debug("%s: I2C Speed(%i), Freq(%i), Clk Source(%i)\n", __func__,
-             setup->speed, setup->speed_freq, setup->clock_src);
-       debug("%s: I2C Rise(%i) and Fall(%i) Time\n", __func__,
-             setup->rise_time, setup->fall_time);
-       debug("%s: I2C Analog Filter(%s), DNF(%i)\n", __func__,
-             setup->analog_filter ? "On" : "Off", setup->dnf);
+       log_debug("I2C Freq(%i), Clk Source(%i)\n",
+                 setup->speed_freq, setup->clock_src);
+       log_debug("I2C Rise(%i) and Fall(%i) Time\n",
+                 setup->rise_time, setup->fall_time);
+       log_debug("I2C Analog Filter(%s), DNF(%i)\n",
+                 setup->analog_filter ? "On" : "Off", setup->dnf);
+
+       i2c_priv->speed = setup->speed_freq;
 
        return 0;
 }
 
+static int stm32_i2c_write_fm_plus_bits(struct stm32_i2c_priv *i2c_priv)
+{
+       int ret;
+       bool enable = i2c_priv->speed > I2C_SPEED_FAST_RATE;
+
+       /* Optional */
+       if (IS_ERR_OR_NULL(i2c_priv->regmap))
+               return 0;
+
+       if (i2c_priv->regmap_sreg == i2c_priv->regmap_creg)
+               ret = regmap_update_bits(i2c_priv->regmap,
+                                        i2c_priv->regmap_sreg,
+                                        i2c_priv->regmap_mask,
+                                        enable ? i2c_priv->regmap_mask : 0);
+       else
+               ret = regmap_write(i2c_priv->regmap,
+                                  enable ? i2c_priv->regmap_sreg :
+                                           i2c_priv->regmap_creg,
+                                  i2c_priv->regmap_mask);
+
+       return ret;
+}
+
 static int stm32_i2c_hw_config(struct stm32_i2c_priv *i2c_priv)
 {
        struct stm32_i2c_regs *regs = i2c_priv->regs;
@@ -755,6 +827,11 @@ static int stm32_i2c_hw_config(struct stm32_i2c_priv *i2c_priv)
        /* Disable I2C */
        clrbits_le32(&regs->cr1, STM32_I2C_CR1_PE);
 
+       /* Setup Fast mode plus if necessary */
+       ret = stm32_i2c_write_fm_plus_bits(i2c_priv);
+       if (ret)
+               return ret;
+
        /* Timing settings */
        timing |= STM32_I2C_TIMINGR_PRESC(t.presc);
        timing |= STM32_I2C_TIMINGR_SCLDEL(t.scldel);
@@ -764,34 +841,31 @@ static int stm32_i2c_hw_config(struct stm32_i2c_priv *i2c_priv)
        writel(timing, &regs->timingr);
 
        /* Enable I2C */
-       if (i2c_priv->setup->analog_filter)
+       if (i2c_priv->setup.analog_filter)
                clrbits_le32(&regs->cr1, STM32_I2C_CR1_ANFOFF);
        else
                setbits_le32(&regs->cr1, STM32_I2C_CR1_ANFOFF);
+
+       /* Program the Digital Filter */
+       clrsetbits_le32(&regs->cr1, STM32_I2C_CR1_DNF_MASK,
+                       STM32_I2C_CR1_DNF(i2c_priv->setup.dnf));
+
        setbits_le32(&regs->cr1, STM32_I2C_CR1_PE);
 
        return 0;
 }
 
-static int stm32_i2c_set_bus_speed(struct udevice *bus, unsigned int speed)
+static int stm32_i2c_set_bus_speed(struct udevice *dev, unsigned int speed)
 {
-       struct stm32_i2c_priv *i2c_priv = dev_get_priv(bus);
+       struct stm32_i2c_priv *i2c_priv = dev_get_priv(dev);
 
-       switch (speed) {
-       case STANDARD_RATE:
-               i2c_priv->speed = STM32_I2C_SPEED_STANDARD;
-               break;
-       case FAST_RATE:
-               i2c_priv->speed = STM32_I2C_SPEED_FAST;
-               break;
-       case FAST_PLUS_RATE:
-               i2c_priv->speed = STM32_I2C_SPEED_FAST_PLUS;
-               break;
-       default:
-               debug("%s: Speed %d not supported\n", __func__, speed);
+       if (speed > I2C_SPEED_FAST_PLUS_RATE) {
+               dev_dbg(dev, "Speed %d not supported\n", speed);
                return -EINVAL;
        }
 
+       i2c_priv->speed = speed;
+
        return stm32_i2c_hw_config(i2c_priv);
 }
 
@@ -834,22 +908,43 @@ clk_free:
        return ret;
 }
 
-static int stm32_ofdata_to_platdata(struct udevice *dev)
+static int stm32_of_to_plat(struct udevice *dev)
 {
+       const struct stm32_i2c_data *data;
        struct stm32_i2c_priv *i2c_priv = dev_get_priv(dev);
        u32 rise_time, fall_time;
+       int ret;
 
-       i2c_priv->setup = (struct stm32_i2c_setup *)dev_get_driver_data(dev);
-       if (!i2c_priv->setup)
+       data = (const struct stm32_i2c_data *)dev_get_driver_data(dev);
+       if (!data)
                return -EINVAL;
 
-       rise_time = dev_read_u32_default(dev, "i2c-scl-rising-time-ns", 0);
-       if (rise_time)
-               i2c_priv->setup->rise_time = rise_time;
+       rise_time = dev_read_u32_default(dev, "i2c-scl-rising-time-ns",
+                                        STM32_I2C_RISE_TIME_DEFAULT);
+
+       fall_time = dev_read_u32_default(dev, "i2c-scl-falling-time-ns",
+                                        STM32_I2C_FALL_TIME_DEFAULT);
 
-       fall_time = dev_read_u32_default(dev, "i2c-scl-falling-time-ns", 0);
-       if (fall_time)
-               i2c_priv->setup->fall_time = fall_time;
+       i2c_priv->dnf_dt = dev_read_u32_default(dev, "i2c-digital-filter-width-ns", 0);
+       if (!dev_read_bool(dev, "i2c-digital-filter"))
+               i2c_priv->dnf_dt = 0;
+
+       i2c_priv->setup.analog_filter = dev_read_bool(dev, "i2c-analog-filter");
+
+       /* Optional */
+       i2c_priv->regmap = syscon_regmap_lookup_by_phandle(dev,
+                                                          "st,syscfg-fmp");
+       if (!IS_ERR(i2c_priv->regmap)) {
+               u32 fmp[3];
+
+               ret = dev_read_u32_array(dev, "st,syscfg-fmp", fmp, 3);
+               if (ret)
+                       return ret;
+
+               i2c_priv->regmap_sreg = fmp[1];
+               i2c_priv->regmap_creg = fmp[1] + data->fmp_clr_offset;
+               i2c_priv->regmap_mask = fmp[2];
+       }
 
        return 0;
 }
@@ -860,7 +955,8 @@ static const struct dm_i2c_ops stm32_i2c_ops = {
 };
 
 static const struct udevice_id stm32_i2c_of_match[] = {
-       { .compatible = "st,stm32f7-i2c", .data = (ulong)&stm32f7_setup },
+       { .compatible = "st,stm32f7-i2c", .data = (ulong)&stm32f7_data },
+       { .compatible = "st,stm32mp15-i2c", .data = (ulong)&stm32mp15_data },
        {}
 };
 
@@ -868,8 +964,8 @@ U_BOOT_DRIVER(stm32f7_i2c) = {
        .name = "stm32f7-i2c",
        .id = UCLASS_I2C,
        .of_match = stm32_i2c_of_match,
-       .ofdata_to_platdata = stm32_ofdata_to_platdata,
+       .of_to_plat = stm32_of_to_plat,
        .probe = stm32_i2c_probe,
-       .priv_auto_alloc_size = sizeof(struct stm32_i2c_priv),
+       .priv_auto      = sizeof(struct stm32_i2c_priv),
        .ops = &stm32_i2c_ops,
 };