From f8fe203c145990d107dac84bf2bf441237bc5b17 Mon Sep 17 00:00:00 2001
From: christophe montaud <christophe.montaud@st.com>
Date: Fri, 4 Jan 2019 15:09:10 +0100
Subject: [PATCH 5/5] ARM v2018.11 stm32mp r1 MISC

---
 .gitignore                                  |    3 +
 MAINTAINERS                                 |    2 +
 Makefile                                    |    2 +-
 arch/sandbox/include/asm/state.h            |    1 +
 cmd/Kconfig                                 |    8 +
 cmd/Makefile                                |    1 +
 cmd/adc.c                                   |   70 +-
 cmd/bmp.c                                   |   18 +-
 cmd/pinmux.c                                |  146 +++
 cmd/pxe.c                                   |   24 +
 cmd/remoteproc.c                            |   62 +-
 cmd/usb_mass_storage.c                      |    3 +
 common/Makefile                             |    1 +
 common/board_f.c                            |    3 +-
 common/image.c                              |    1 +
 common/spl/spl_spi.c                        |    9 +-
 common/usb.c                                |    2 +
 drivers/Kconfig                             |    2 +
 drivers/Makefile                            |    1 +
 drivers/adc/adc-uclass.c                    |   54 +-
 drivers/clk/clk_stm32mp1.c                  |  271 ++++-
 drivers/core/syscon-uclass.c                |   61 +-
 drivers/core/uclass.c                       |   13 +
 drivers/dfu/Kconfig                         |    5 +
 drivers/dfu/Makefile                        |    1 +
 drivers/dfu/dfu.c                           |   84 +-
 drivers/dfu/dfu_sf.c                        |   54 +-
 drivers/dfu/dfu_virt.c                      |   47 +
 drivers/gpio/stm32f7_gpio.c                 |  120 ++-
 drivers/hwspinlock/Kconfig                  |   24 +
 drivers/hwspinlock/Makefile                 |    7 +
 drivers/hwspinlock/hwspinlock-uclass.c      |  144 +++
 drivers/hwspinlock/sandbox_hwspinlock.c     |   56 +
 drivers/hwspinlock/stm32_hwspinlock.c       |   90 ++
 drivers/i2c/stm32f7_i2c.c                   |   41 +-
 drivers/mailbox/Kconfig                     |    7 +
 drivers/mailbox/Makefile                    |    1 +
 drivers/mailbox/stm32-ipcc.c                |  187 ++++
 drivers/misc/stm32mp_fuse.c                 |   28 +
 drivers/mmc/mmc_write.c                     |    2 +-
 drivers/mmc/stm32_sdmmc2.c                  |   67 +-
 drivers/mtd/nand/raw/Kconfig                |   11 +
 drivers/mtd/nand/raw/Makefile               |    1 +
 drivers/mtd/nand/raw/nand_ids.c             |    4 +
 drivers/mtd/nand/raw/stm32_fmc2_nand.c      | 1092 +++++++++++++++++++
 drivers/mtd/spi/spi_flash.c                 |    9 +
 drivers/net/dwc_eth_qos.c                   |  451 ++++++--
 drivers/phy/phy-stm32-usbphyc.c             |  201 ++--
 drivers/pinctrl/Kconfig                     |   19 +
 drivers/pinctrl/Makefile                    |    1 +
 drivers/pinctrl/pinctrl-sandbox.c           |   18 +
 drivers/pinctrl/pinctrl-stmfx.c             |  414 ++++++++
 drivers/pinctrl/pinctrl-uclass.c            |   39 +-
 drivers/pinctrl/pinctrl_stm32.c             |  252 ++++-
 drivers/power/pmic/Kconfig                  |    6 +-
 drivers/power/pmic/Makefile                 |    2 +-
 drivers/power/pmic/stpmic1.c                |  258 +++++
 drivers/power/pmic/stpmu1.c                 |   95 --
 drivers/power/regulator/Kconfig             |   14 +-
 drivers/power/regulator/Makefile            |    2 +-
 drivers/power/regulator/fixed.c             |    4 +-
 drivers/power/regulator/regulator-uclass.c  |    5 +
 drivers/power/regulator/stpmic1.c           |  672 ++++++++++++
 drivers/power/regulator/stpmu1.c            |  671 ------------
 drivers/ram/stm32mp1/Kconfig                |   29 +
 drivers/ram/stm32mp1/Makefile               |    7 +
 drivers/ram/stm32mp1/stm32mp1_ddr.c         |  522 +++++++++-
 drivers/ram/stm32mp1/stm32mp1_ddr.h         |   10 +-
 drivers/ram/stm32mp1/stm32mp1_ddr_regs.h    |    3 +
 drivers/ram/stm32mp1/stm32mp1_interactive.c |  467 +++++++++
 drivers/ram/stm32mp1/stm32mp1_ram.c         |   30 +-
 drivers/ram/stm32mp1/stm32mp1_tests.c       | 1360 ++++++++++++++++++++++++
 drivers/ram/stm32mp1/stm32mp1_tests.h       |   34 +
 drivers/ram/stm32mp1/stm32mp1_tuning.c      | 1504 +++++++++++++++++++++++++++
 drivers/ram/stm32mp1/stm32mp1_tuning.h      |   54 +
 drivers/remoteproc/Kconfig                  |    9 +
 drivers/remoteproc/Makefile                 |    1 +
 drivers/remoteproc/rproc-uclass.c           |  333 +++++-
 drivers/remoteproc/stm32_copro.c            |  272 +++++
 drivers/reset/stm32-reset.c                 |   33 +-
 drivers/serial/serial_stm32.c               |   18 +-
 drivers/spi/Kconfig                         |    2 +-
 drivers/spi/soft_spi.c                      |    4 +-
 drivers/sysreset/sysreset_syscon.c          |   18 +-
 drivers/usb/gadget/Kconfig                  |    9 +
 drivers/usb/gadget/Makefile                 |    1 +
 drivers/usb/gadget/dwc2_udc_otg.c           |    3 +
 drivers/usb/gadget/g_dnl.c                  |    5 +
 drivers/usb/gadget/gen_udc_otg_phy.c        |   66 ++
 drivers/usb/host/dwc2.c                     |  118 ++-
 drivers/video/Kconfig                       |   33 +
 drivers/video/Makefile                      |    4 +
 drivers/video/dw_mipi_dsi.c                 |  826 +++++++++++++++
 drivers/video/mipi_display.c                |  817 +++++++++++++++
 drivers/video/orisetech_otm8009a.c          |  339 ++++++
 drivers/video/raydium-rm68200.c             |  338 ++++++
 drivers/video/stm32/Kconfig                 |    9 +
 drivers/video/stm32/Makefile                |    1 +
 drivers/video/stm32/stm32_dsi.c             |  446 ++++++++
 drivers/video/stm32/stm32_ltdc.c            |  155 +--
 drivers/video/video-uclass.c                |   14 +
 drivers/watchdog/Kconfig                    |   15 +
 drivers/watchdog/Makefile                   |    1 +
 drivers/watchdog/stm32mp_wdt.c              |  119 +++
 include/adc.h                               |   21 +
 include/configs/stm32mp1.h                  |  117 ++-
 include/dfu.h                               |   29 +
 include/dm/pinctrl.h                        |   59 ++
 include/dm/uclass-id.h                      |    1 +
 include/dm/uclass.h                         |   28 +
 include/dw_mipi_dsi.h                       |   32 +
 include/g_dnl.h                             |    1 +
 include/hwspinlock.h                        |  140 +++
 include/image.h                             |    1 +
 include/mipi_display.h                      |  257 ++++-
 include/power/stpmic1.h                     |  118 +++
 include/power/stpmu1.h                      |   85 --
 include/remoteproc.h                        |   20 +-
 include/syscon.h                            |    9 +
 include/usb/dwc2_udc.h                      |    1 +
 test/dm/Makefile                            |    1 +
 test/dm/hwspinlock.c                        |   40 +
 test/py/tests/test_pinmux.py                |   62 ++
 tools/stm32image.c                          |    8 +-
 124 files changed, 13670 insertions(+), 1353 deletions(-)
 create mode 100644 cmd/pinmux.c
 create mode 100644 drivers/dfu/dfu_virt.c
 create mode 100644 drivers/hwspinlock/Kconfig
 create mode 100644 drivers/hwspinlock/Makefile
 create mode 100644 drivers/hwspinlock/hwspinlock-uclass.c
 create mode 100644 drivers/hwspinlock/sandbox_hwspinlock.c
 create mode 100644 drivers/hwspinlock/stm32_hwspinlock.c
 create mode 100644 drivers/mailbox/stm32-ipcc.c
 create mode 100644 drivers/mtd/nand/raw/stm32_fmc2_nand.c
 create mode 100644 drivers/pinctrl/pinctrl-stmfx.c
 create mode 100644 drivers/power/pmic/stpmic1.c
 delete mode 100644 drivers/power/pmic/stpmu1.c
 create mode 100644 drivers/power/regulator/stpmic1.c
 delete mode 100644 drivers/power/regulator/stpmu1.c
 create mode 100644 drivers/ram/stm32mp1/stm32mp1_interactive.c
 create mode 100644 drivers/ram/stm32mp1/stm32mp1_tests.c
 create mode 100644 drivers/ram/stm32mp1/stm32mp1_tests.h
 create mode 100644 drivers/ram/stm32mp1/stm32mp1_tuning.c
 create mode 100644 drivers/ram/stm32mp1/stm32mp1_tuning.h
 create mode 100644 drivers/remoteproc/stm32_copro.c
 create mode 100644 drivers/usb/gadget/gen_udc_otg_phy.c
 create mode 100644 drivers/video/dw_mipi_dsi.c
 create mode 100644 drivers/video/mipi_display.c
 create mode 100644 drivers/video/orisetech_otm8009a.c
 create mode 100644 drivers/video/raydium-rm68200.c
 create mode 100644 drivers/video/stm32/stm32_dsi.c
 create mode 100644 drivers/watchdog/stm32mp_wdt.c
 create mode 100644 include/dw_mipi_dsi.h
 create mode 100644 include/hwspinlock.h
 create mode 100644 include/power/stpmic1.h
 delete mode 100644 include/power/stpmu1.h
 create mode 100644 test/dm/hwspinlock.c
 create mode 100644 test/py/tests/test_pinmux.py

diff --git a/.gitignore b/.gitignore
index 8d18d6f..b22e7d0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -88,3 +88,6 @@ GTAGS
 *.orig
 *~
 \#*#
+
+/oe-*
+bitbake-cookerdaemon.log
diff --git a/MAINTAINERS b/MAINTAINERS
index abdb6dc..17f9cd3 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -242,7 +242,9 @@ F:	drivers/misc/stm32mp_fuse.c
 F:	drivers/mmc/stm32_sdmmc2.c
 F:	drivers/phy/phy-stm32-usbphyc.c
 F:	drivers/pinctrl/pinctrl_stm32.c
+F:	drivers/power/pmic/stpmic1.c
 F:	drivers/power/regulator/stm32-vrefbuf.c
+F:	drivers/power/regulator/stpmic1.c
 F:	drivers/ram/stm32mp1/
 F:	drivers/misc/stm32_rcc.c
 F:	drivers/reset/stm32-reset.c
diff --git a/Makefile b/Makefile
index 552687d..84cb372 100644
--- a/Makefile
+++ b/Makefile
@@ -3,7 +3,7 @@
 VERSION = 2018
 PATCHLEVEL = 11
 SUBLEVEL =
-EXTRAVERSION =
+EXTRAVERSION = -stm32mp-r1
 NAME =
 
 # *DOCUMENTATION*
diff --git a/arch/sandbox/include/asm/state.h b/arch/sandbox/include/asm/state.h
index dcb6d5f..79e0e1e 100644
--- a/arch/sandbox/include/asm/state.h
+++ b/arch/sandbox/include/asm/state.h
@@ -97,6 +97,7 @@ struct sandbox_state {
 	/* Information about Watchdog */
 	struct sandbox_wdt_info wdt;
 
+	bool hwspinlock;		/* Hardware Spinlock status */
 	ulong next_tag;			/* Next address tag to allocate */
 	struct list_head mapmem_head;	/* struct sandbox_mapmem_entry */
 };
diff --git a/cmd/Kconfig b/cmd/Kconfig
index ad14c9c..3aa10d5 100644
--- a/cmd/Kconfig
+++ b/cmd/Kconfig
@@ -953,6 +953,14 @@ config CMD_PCMCIA
 	  about 1990. These devices are typically removable memory or network
 	  cards using a standard 68-pin connector.
 
+config CMD_PINMUX
+	bool "pinmux - show pins muxing"
+	default y if PINCTRL
+	help
+	  Parse all available pin-controllers and show pins muxing. This
+	  is useful for debug purpoer to check the pin muxing and to know if
+	  a pin is configured as a GPIO or as an alternate function.
+
 config CMD_POWEROFF
 	bool "poweroff"
 	help
diff --git a/cmd/Makefile b/cmd/Makefile
index ac4830a..e9ab126 100644
--- a/cmd/Makefile
+++ b/cmd/Makefile
@@ -103,6 +103,7 @@ ifdef CONFIG_PCI
 obj-$(CONFIG_CMD_PCI) += pci.o
 endif
 obj-y += pcmcia.o
+obj-$(CONFIG_CMD_PINMUX) += pinmux.o
 obj-$(CONFIG_CMD_PXE) += pxe.o
 obj-$(CONFIG_CMD_WOL) += wol.o
 obj-$(CONFIG_CMD_QFW) += qfw.o
diff --git a/cmd/adc.c b/cmd/adc.c
index c8857ed..2d635ac 100644
--- a/cmd/adc.c
+++ b/cmd/adc.c
@@ -35,7 +35,7 @@ static int do_adc_info(cmd_tbl_t *cmdtp, int flag, int argc,
 		       char *const argv[])
 {
 	struct udevice *dev;
-	unsigned int data_mask;
+	unsigned int data_mask, ch_mask;
 	int ret, vss, vdd;
 
 	if (argc < 2)
@@ -49,6 +49,10 @@ static int do_adc_info(cmd_tbl_t *cmdtp, int flag, int argc,
 
 	printf("ADC Device '%s' :\n", argv[1]);
 
+	ret = adc_channel_mask(dev, &ch_mask);
+	if (!ret)
+		printf("channel mask: %x\n", ch_mask);
+
 	ret = adc_data_mask(dev, &data_mask);
 	if (!ret)
 		printf("data mask: %x\n", data_mask);
@@ -67,8 +71,9 @@ static int do_adc_info(cmd_tbl_t *cmdtp, int flag, int argc,
 static int do_adc_single(cmd_tbl_t *cmdtp, int flag, int argc,
 			 char *const argv[])
 {
+	struct udevice *dev;
 	unsigned int data;
-	int ret;
+	int ret, uV;
 
 	if (argc < 3)
 		return CMD_RET_USAGE;
@@ -81,7 +86,62 @@ static int do_adc_single(cmd_tbl_t *cmdtp, int flag, int argc,
 		return CMD_RET_FAILURE;
 	}
 
-	printf("%u\n", data);
+	ret = uclass_get_device_by_name(UCLASS_ADC, argv[1], &dev);
+	if (!ret && !adc_raw_to_uV(dev, data, &uV))
+		printf("%u, %d uV\n", data, uV);
+	else
+		printf("%u\n", data);
+
+	return CMD_RET_SUCCESS;
+}
+
+static int do_adc_scan(cmd_tbl_t *cmdtp, int flag, int argc,
+		       char *const argv[])
+{
+	struct adc_channel ch[ADC_MAX_CHANNEL];
+	struct udevice *dev;
+	unsigned int ch_mask;
+	int i, chan, ret, uV;
+
+	if (argc < 2)
+		return CMD_RET_USAGE;
+
+	ret = uclass_get_device_by_name(UCLASS_ADC, argv[1], &dev);
+	if (ret) {
+		pr_err("Can't get the ADC %s: %d\n", argv[1], ret);
+		return CMD_RET_FAILURE;
+	}
+
+	switch (argc) {
+	case 3:
+		ch_mask = simple_strtoul(argv[2], NULL, 0);
+		if (ch_mask)
+			break;
+	case 2:
+		ret = adc_channel_mask(dev, &ch_mask);
+		if (ret) {
+			pr_err("Can't get mask for %s: %d\n", dev->name, ret);
+			return CMD_RET_FAILURE;
+		}
+		break;
+	}
+
+	ret = adc_channels_single_shot(dev->name, ch_mask, ch);
+	if (ret) {
+		pr_err("Can't get single shot for %s (chans mask: 0x%x): %d\n",
+		       dev->name, ch_mask, ret);
+		return CMD_RET_FAILURE;
+	}
+
+	for (chan = 0, i = 0; chan < ADC_MAX_CHANNEL; chan++) {
+		if (!(ch_mask & ADC_CHANNEL(chan)))
+			continue;
+		if (!adc_raw_to_uV(dev, ch[i].data, &uV))
+			printf("[%02d]: %u, %d uV\n", ch[i].id, ch[i].data, uV);
+		else
+			printf("[%02d]: %u\n", ch[i].id, ch[i].data);
+		i++;
+	}
 
 	return CMD_RET_SUCCESS;
 }
@@ -90,6 +150,7 @@ static cmd_tbl_t cmd_adc_sub[] = {
 	U_BOOT_CMD_MKENT(list, 1, 1, do_adc_list, "", ""),
 	U_BOOT_CMD_MKENT(info, 2, 1, do_adc_info, "", ""),
 	U_BOOT_CMD_MKENT(single, 3, 1, do_adc_single, "", ""),
+	U_BOOT_CMD_MKENT(scan, 3, 1, do_adc_scan, "", ""),
 };
 
 static int do_adc(cmd_tbl_t *cmdtp, int flag, int argc,
@@ -115,6 +176,7 @@ static int do_adc(cmd_tbl_t *cmdtp, int flag, int argc,
 static char adc_help_text[] =
 	"list - list ADC devices\n"
 	"adc info <name> - Get ADC device info\n"
-	"adc single <name> <channel> - Get Single data of ADC device channel";
+	"adc single <name> <channel> - Get Single data of ADC device channel\n"
+	"adc scan <name> [channel mask] - Scan all [or masked] ADC channels";
 
 U_BOOT_CMD(adc, 4, 1, do_adc, "ADC sub-system", adc_help_text);
diff --git a/cmd/bmp.c b/cmd/bmp.c
index 02bdf48..b8af784 100644
--- a/cmd/bmp.c
+++ b/cmd/bmp.c
@@ -124,8 +124,14 @@ static int do_bmp_display(cmd_tbl_t * cmdtp, int flag, int argc, char * const ar
 		break;
 	case 4:
 		addr = simple_strtoul(argv[1], NULL, 16);
-		x = simple_strtoul(argv[2], NULL, 10);
-		y = simple_strtoul(argv[3], NULL, 10);
+		if (!strcmp(argv[2], "m"))
+			x = BMP_ALIGN_CENTER;
+		else
+			x = simple_strtoul(argv[2], NULL, 10);
+		if (!strcmp(argv[3], "m"))
+			y = BMP_ALIGN_CENTER;
+		else
+			y = simple_strtoul(argv[3], NULL, 10);
 		break;
 	default:
 		return CMD_RET_USAGE;
@@ -249,9 +255,11 @@ int bmp_display(ulong addr, int x, int y)
 	if (!ret) {
 		bool align = false;
 
-# ifdef CONFIG_SPLASH_SCREEN_ALIGN
-		align = true;
-# endif /* CONFIG_SPLASH_SCREEN_ALIGN */
+		if (CONFIG_IS_ENABLED(SPLASH_SCREEN_ALIGN) ||
+		    x == BMP_ALIGN_CENTER ||
+		    y == BMP_ALIGN_CENTER)
+			align = true;
+
 		ret = video_bmp_display(dev, addr, x, y, align);
 	}
 #elif defined(CONFIG_LCD)
diff --git a/cmd/pinmux.c b/cmd/pinmux.c
new file mode 100644
index 0000000..6c8ec51
--- /dev/null
+++ b/cmd/pinmux.c
@@ -0,0 +1,146 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <command.h>
+#include <dm.h>
+#include <errno.h>
+#include <dm/pinctrl.h>
+#include <dm/uclass-internal.h>
+
+#define LIMIT_DEVNAME	30
+
+static struct udevice *currdev;
+
+static int do_dev(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	const char *name;
+	int ret;
+
+	switch (argc) {
+	case 2:
+		name = argv[1];
+		ret = uclass_get_device_by_name(UCLASS_PINCTRL, name, &currdev);
+		if (ret) {
+			printf("Can't get the pin-controller: %s!\n", name);
+			return CMD_RET_FAILURE;
+		}
+	case 1:
+		if (!currdev) {
+			printf("Pin-controller device is not set!\n");
+			return CMD_RET_USAGE;
+		}
+
+		printf("dev: %s\n", currdev->name);
+	}
+
+	return CMD_RET_SUCCESS;
+}
+
+static int show_pinmux(struct udevice *dev)
+{
+	char pin_name[PINNAME_SIZE];
+	char pin_mux[PINMUX_SIZE];
+	int pins_count;
+	int i;
+	int ret;
+
+	pins_count = pinctrl_get_pins_count(dev);
+
+	if (pins_count == -ENOSYS) {
+		printf("Ops get_pins_count not supported\n");
+		return pins_count;
+	}
+
+	for (i = 0; i < pins_count; i++) {
+		ret = pinctrl_get_pin_name(dev, i, pin_name, PINNAME_SIZE);
+		if (ret == -ENOSYS) {
+			printf("Ops get_pin_name not supported\n");
+			return ret;
+		}
+
+		ret = pinctrl_get_pin_muxing(dev, i, pin_mux, PINMUX_SIZE);
+		if (ret) {
+			printf("Ops get_pin_muxing error (%d)\n", ret);
+			return ret;
+		}
+
+		printf("%-*s: %-*s\n", PINNAME_SIZE, pin_name,
+		       PINMUX_SIZE, pin_mux);
+	}
+
+	return 0;
+}
+
+static int do_status(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	struct udevice *dev;
+	int ret = CMD_RET_USAGE;
+
+	if (currdev && (argc < 2 || strcmp(argv[1], "-a")))
+		return show_pinmux(currdev);
+
+	if (argc < 2 || strcmp(argv[1], "-a"))
+		return ret;
+
+	uclass_foreach_dev_probe(UCLASS_PINCTRL, dev) {
+		/* insert a separator between each pin-controller display */
+		printf("--------------------------\n");
+		printf("%s:\n", dev->name);
+		ret = show_pinmux(dev);
+		if (ret < 0)
+			printf("Can't display pin muxing for %s\n",
+			       dev->name);
+	}
+
+	return ret;
+}
+
+static int do_list(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	struct udevice *dev;
+
+	printf("| %-*.*s| %-*.*s| %s\n",
+	       LIMIT_DEVNAME, LIMIT_DEVNAME, "Device",
+	       LIMIT_DEVNAME, LIMIT_DEVNAME, "Driver",
+	       "Parent");
+
+	uclass_foreach_dev_probe(UCLASS_PINCTRL, dev) {
+		printf("| %-*.*s| %-*.*s| %s\n",
+		       LIMIT_DEVNAME, LIMIT_DEVNAME, dev->name,
+		       LIMIT_DEVNAME, LIMIT_DEVNAME, dev->driver->name,
+		       dev->parent->name);
+	}
+
+	return CMD_RET_SUCCESS;
+}
+
+static cmd_tbl_t pinmux_subcmd[] = {
+	U_BOOT_CMD_MKENT(dev, 2, 1, do_dev, "", ""),
+	U_BOOT_CMD_MKENT(list, 1, 1, do_list, "", ""),
+	U_BOOT_CMD_MKENT(status, 2, 1, do_status, "", ""),
+};
+
+static int do_pinmux(cmd_tbl_t *cmdtp, int flag, int argc,
+		     char * const argv[])
+{
+	cmd_tbl_t *cmd;
+
+	argc--;
+	argv++;
+
+	cmd = find_cmd_tbl(argv[0], pinmux_subcmd, ARRAY_SIZE(pinmux_subcmd));
+	if (!cmd || argc > cmd->maxargs)
+		return CMD_RET_USAGE;
+
+	return cmd->cmd(cmdtp, flag, argc, argv);
+}
+
+U_BOOT_CMD(pinmux, CONFIG_SYS_MAXARGS, 1, do_pinmux,
+	   "show pin-controller muxing",
+	   "list                     - list UCLASS_PINCTRL devices\n"
+	   "pinmux dev [pincontroller-name] - select pin-controller device\n"
+	   "pinmux status [-a]              - print pin-controller muxing [for all]\n"
+)
diff --git a/cmd/pxe.c b/cmd/pxe.c
index 2745553..e777702 100644
--- a/cmd/pxe.c
+++ b/cmd/pxe.c
@@ -8,11 +8,13 @@
 #include <command.h>
 #include <malloc.h>
 #include <mapmem.h>
+#include <lcd.h>
 #include <linux/string.h>
 #include <linux/ctype.h>
 #include <errno.h>
 #include <linux/list.h>
 #include <fs.h>
+#include <splash.h>
 #include <asm/io.h>
 
 #include "menu.h"
@@ -488,6 +490,7 @@ struct pxe_label {
  *
  * title - the name of the menu as given by a 'menu title' line.
  * default_label - the name of the default label, if any.
+ * bmp - the bmp file name which is displayed in background
  * timeout - time in tenths of a second to wait for a user key-press before
  *           booting the default label.
  * prompt - if 0, don't prompt for a choice unless the timeout period is
@@ -498,6 +501,7 @@ struct pxe_label {
 struct pxe_menu {
 	char *title;
 	char *default_label;
+	char *bmp;
 	int timeout;
 	int prompt;
 	struct list_head labels;
@@ -850,6 +854,7 @@ enum token_type {
 	T_FDTDIR,
 	T_ONTIMEOUT,
 	T_IPAPPEND,
+	T_BACKGROUND,
 	T_INVALID
 };
 
@@ -883,6 +888,7 @@ static const struct token keywords[] = {
 	{"fdtdir", T_FDTDIR},
 	{"ontimeout", T_ONTIMEOUT,},
 	{"ipappend", T_IPAPPEND,},
+	{"background", T_BACKGROUND,},
 	{NULL, T_INVALID}
 };
 
@@ -1160,6 +1166,10 @@ static int parse_menu(cmd_tbl_t *cmdtp, char **c, struct pxe_menu *cfg,
 						nest_level + 1);
 		break;
 
+	case T_BACKGROUND:
+		err = parse_sliteral(c, &cfg->bmp);
+		break;
+
 	default:
 		printf("Ignoring malformed menu command: %.*s\n",
 				(int)(*c - s), s);
@@ -1574,6 +1584,20 @@ static void handle_pxe_menu(cmd_tbl_t *cmdtp, struct pxe_menu *cfg)
 	struct menu *m;
 	int err;
 
+#ifdef CONFIG_CMD_BMP
+	/* display BMP if available */
+	if (cfg->bmp) {
+		if (get_relfile(cmdtp, cfg->bmp, load_addr)) {
+			run_command("cls", 0);
+			bmp_display(load_addr,
+				    BMP_ALIGN_CENTER, BMP_ALIGN_CENTER);
+		} else {
+			printf("Skipping background bmp %s for failure\n",
+			       cfg->bmp);
+		}
+	}
+#endif
+
 	m = pxe_menu_to_menu(cfg);
 	if (!m)
 		return;
diff --git a/cmd/remoteproc.c b/cmd/remoteproc.c
index 81463f3..755e933 100644
--- a/cmd/remoteproc.c
+++ b/cmd/remoteproc.c
@@ -103,7 +103,7 @@ static int do_remoteproc_list(cmd_tbl_t *cmdtp, int flag, int argc,
 }
 
 /**
- * do_remoteproc_load() - Load a remote processor with binary image
+ * do_remoteproc_load() - Load a remote processor with binary or elf image
  * @cmdtp:	unused
  * @flag:	unused
  * @argc:	argument count for the load function
@@ -143,6 +143,53 @@ static int do_remoteproc_load(cmd_tbl_t *cmdtp, int flag, int argc,
 }
 
 /**
+ * do_remoteproc_load_rsc_table() - Get resource table from an elf image
+ * @cmdtp:	unused
+ * @flag:	unused
+ * @argc:	argument count for the load function
+ * @argv:	arguments for the load function
+ *
+ * Return: 0 if no error, else returns appropriate error value.
+ */
+static int do_remoteproc_load_rsc_table(cmd_tbl_t *cmdtp, int flag, int argc,
+					char *const argv[])
+{
+	ulong addr, size, rsc_addr;
+	unsigned int rsc_size;
+	int id, ret;
+
+	if (argc != 4)
+		return CMD_RET_USAGE;
+
+	id = (int)simple_strtoul(argv[1], NULL, 3);
+	addr = simple_strtoul(argv[2], NULL, 16);
+
+	size = simple_strtoul(argv[3], NULL, 16);
+
+	if (!size) {
+		printf("\t Expect some size??\n");
+		return CMD_RET_USAGE;
+	}
+
+	if (!rproc_is_initialized()) {
+		printf("\tRemote Processors are not initialized\n");
+		return CMD_RET_USAGE;
+	}
+
+	ret = rproc_load_rsc_table(id, addr, size, &rsc_addr, &rsc_size);
+	if (!ret) {
+		env_set_hex("copro_rsc_addr", rsc_addr);
+		env_set_hex("copro_rsc_size", rsc_size);
+	}
+
+	printf("Remote Processor %d resource table %s : 0x%08lx-0x%x\n",
+	       id, ret ? "Not found" : "Found", ret ? 0 : rsc_addr,
+	       ret ? 0 : rsc_size);
+
+	return ret ? CMD_RET_FAILURE : 0;
+}
+
+/**
  * do_remoteproc_wrapper() - wrapper for various  rproc commands
  * @cmdtp:	unused
  * @flag:	unused
@@ -172,6 +219,9 @@ static int do_remoteproc_wrapper(cmd_tbl_t *cmdtp, int flag, int argc,
 
 	if (!strcmp(argv[0], "start")) {
 		ret = rproc_start(id);
+		if (!ret)
+			env_set("copro_state", "booted");
+
 	} else if (!strcmp(argv[0], "stop")) {
 		ret = rproc_stop(id);
 	} else if (!strcmp(argv[0], "reset")) {
@@ -213,6 +263,12 @@ static cmd_tbl_t cmd_remoteproc_sub[] = {
 			 "- id: ID of the remote processor(see 'list' cmd)\n"
 			 "- addr: Address in memory of the image to loadup\n"
 			 "- size: Size of the image to loadup\n"),
+	U_BOOT_CMD_MKENT(load_rsc, 5, 1, do_remoteproc_load_rsc_table,
+			 "Load resource table address from remote processor provided image",
+			 "<id> [addr] [size]\n"
+			 "- id: ID of the remote processor(see 'list' cmd)\n"
+			 "- addr: Address in memory of the image\n"
+			 "- size: Size of the image\n"),
 	U_BOOT_CMD_MKENT(start, 1, 1, do_remoteproc_wrapper,
 			 "Start remote processor",
 			 "id - ID of the remote processor (see 'list' cmd)\n"),
@@ -272,8 +328,10 @@ U_BOOT_CMD(rproc, 5, 1, do_remoteproc,
 	   "\n\tSubcommands:\n"
 	   "\tinit   - Enumerate and initalize the remote processors\n"
 	   "\tlist   - list available remote processors\n"
-	   "\tload <id> [addr] [size]- Load the remote processor with binary\n"
+	   "\tload <id> [addr] [size]- Load the remote processor with\n"
 	   "\t		  image stored at address [addr] in memory\n"
+	   "\tload_rsc <id> [addr] [size]- Load resource table from remote\n"
+	   "\t		  processor provided image at address [addr]\n"
 	   "\tstart <id>	- Start the remote processor(must be loaded)\n"
 	   "\tstop <id>	- Stop the remote processor\n"
 	   "\treset <id>	- Reset the remote processor\n"
diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
index 0d55114..26b41b4c4 100644
--- a/cmd/usb_mass_storage.c
+++ b/cmd/usb_mass_storage.c
@@ -14,6 +14,7 @@
 #include <part.h>
 #include <usb.h>
 #include <usb_mass_storage.h>
+#include <watchdog.h>
 
 static int ums_read_sector(struct ums *ums_dev,
 			   ulong start, lbaint_t blkcnt, void *buf)
@@ -226,6 +227,8 @@ static int do_usb_mass_storage(cmd_tbl_t *cmdtp, int flag,
 			rc = CMD_RET_SUCCESS;
 			goto cleanup_register;
 		}
+
+		WATCHDOG_RESET();
 	}
 
 cleanup_register:
diff --git a/common/Makefile b/common/Makefile
index a238836..336bed7 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -116,6 +116,7 @@ endif
 
 obj-y += cli.o
 obj-$(CONFIG_FSL_DDR_INTERACTIVE) += cli_simple.o cli_readline.o
+obj-$(CONFIG_STM32MP1_DDR_INTERACTIVE) += cli_simple.o cli_readline.o
 obj-$(CONFIG_DFU_OVER_USB) += dfu.o
 obj-y += command.o
 obj-$(CONFIG_$(SPL_)LOG) += log.o
diff --git a/common/board_f.c b/common/board_f.c
index afafec5..59745d5 100644
--- a/common/board_f.c
+++ b/common/board_f.c
@@ -92,7 +92,7 @@ static int init_func_watchdog_init(void)
 	(defined(CONFIG_M68K) || defined(CONFIG_MICROBLAZE) || \
 	defined(CONFIG_SH) || defined(CONFIG_AT91SAM9_WATCHDOG) || \
 	defined(CONFIG_DESIGNWARE_WATCHDOG) || \
-	defined(CONFIG_IMX_WATCHDOG))
+	defined(CONFIG_IMX_WATCHDOG) || defined(CONFIG_STM32MP_WATCHDOG))
 	hw_watchdog_init();
 	puts("       Watchdog enabled\n");
 # endif
@@ -434,7 +434,6 @@ static int reserve_uboot(void)
 		debug("Reserving %ldk for U-Boot at: %08lx\n",
 		      gd->mon_len >> 10, gd->relocaddr);
 	}
-
 	gd->start_addr_sp = gd->relocaddr;
 
 	return 0;
diff --git a/common/image.c b/common/image.c
index 1c3a772..6ace71f 100644
--- a/common/image.c
+++ b/common/image.c
@@ -166,6 +166,7 @@ static const table_entry_t uimage_type[] = {
 	{	IH_TYPE_FIRMWARE_IVT, "firmware_ivt", "Firmware with HABv4 IVT" },
 	{       IH_TYPE_PMMC,        "pmmc",        "TI Power Management Micro-Controller Firmware",},
 	{	IH_TYPE_STM32IMAGE, "stm32image", "STMicroelectronics STM32 Image" },
+	{	IH_TYPE_STM32COPRO, "stm32copro", "STMicroelectronics STM32 Coprocessor Image"},
 	{	-1,		    "",		  "",			},
 };
 
diff --git a/common/spl/spl_spi.c b/common/spl/spl_spi.c
index 8cd4830..3cefc9a 100644
--- a/common/spl/spl_spi.c
+++ b/common/spl/spl_spi.c
@@ -78,11 +78,18 @@ static int spl_spi_load_image(struct spl_image_info *spl_image,
 	/*
 	 * Load U-Boot image from SPI flash into RAM
 	 */
-
+#ifdef CONFIG_DM_SPI_FLASH
+	/* In DM mode defaults will be taken from DT */
+	flash = spi_flash_probe(CONFIG_SF_DEFAULT_BUS,
+				CONFIG_SF_DEFAULT_CS,
+				0,
+				0);
+#else
 	flash = spi_flash_probe(CONFIG_SF_DEFAULT_BUS,
 				CONFIG_SF_DEFAULT_CS,
 				CONFIG_SF_DEFAULT_SPEED,
 				CONFIG_SF_DEFAULT_MODE);
+#endif
 	if (!flash) {
 		puts("SPI probe failed.\n");
 		return -ENODEV;
diff --git a/common/usb.c b/common/usb.c
index 78178c5..239b104 100644
--- a/common/usb.c
+++ b/common/usb.c
@@ -233,6 +233,8 @@ int usb_control_msg(struct usb_device *dev, unsigned int pipe,
 	      request, requesttype, value, index, size);
 	dev->status = USB_ST_NOT_PROC; /*not yet processed */
 
+	mdelay(5);
+
 	err = submit_control_msg(dev, pipe, data, size, setup_packet);
 	if (err < 0)
 		return err;
diff --git a/drivers/Kconfig b/drivers/Kconfig
index 927a2b8..7e6ca7c 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -40,6 +40,8 @@ source "drivers/fpga/Kconfig"
 
 source "drivers/gpio/Kconfig"
 
+source "drivers/hwspinlock/Kconfig"
+
 source "drivers/i2c/Kconfig"
 
 source "drivers/input/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index fb38b67..0ef56fb 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -111,4 +111,5 @@ obj-$(CONFIG_W1) += w1/
 obj-$(CONFIG_W1_EEPROM) += w1-eeprom/
 
 obj-$(CONFIG_MACH_PIC32) += ddr/microchip/
+obj-$(CONFIG_DM_HWSPINLOCK) += hwspinlock/
 endif
diff --git a/drivers/adc/adc-uclass.c b/drivers/adc/adc-uclass.c
index 738c1ea..27b2654 100644
--- a/drivers/adc/adc-uclass.c
+++ b/drivers/adc/adc-uclass.c
@@ -6,6 +6,7 @@
 
 #include <common.h>
 #include <errno.h>
+#include <div64.h>
 #include <dm.h>
 #include <dm/lists.h>
 #include <dm/device-internal.h>
@@ -77,6 +78,18 @@ int adc_data_mask(struct udevice *dev, unsigned int *data_mask)
 	return 0;
 }
 
+int adc_channel_mask(struct udevice *dev, unsigned int *channel_mask)
+{
+	struct adc_uclass_platdata *uc_pdata = dev_get_uclass_platdata(dev);
+
+	if (!uc_pdata)
+		return -ENOSYS;
+
+	*channel_mask = uc_pdata->channel_mask;
+
+	return 0;
+}
+
 int adc_stop(struct udevice *dev)
 {
 	const struct adc_ops *ops = dev_get_driver_ops(dev);
@@ -264,8 +277,13 @@ static int adc_vdd_platdata_update(struct udevice *dev)
 	 * will bind before its supply regulator device, then the below 'get'
 	 * will return an error.
 	 */
-	if (!uc_pdata->vdd_supply)
-		return 0;
+	if (!uc_pdata->vdd_supply) {
+		/* Only get vdd_supply once */
+		ret = device_get_supply_regulator(dev, "vdd-supply",
+						  &uc_pdata->vdd_supply);
+		if (ret)
+			return ret;
+	}
 
 	ret = regulator_get_value(uc_pdata->vdd_supply);
 	if (ret < 0)
@@ -281,8 +299,12 @@ static int adc_vss_platdata_update(struct udevice *dev)
 	struct adc_uclass_platdata *uc_pdata = dev_get_uclass_platdata(dev);
 	int ret;
 
-	if (!uc_pdata->vss_supply)
-		return 0;
+	if (!uc_pdata->vss_supply) {
+		ret = device_get_supply_regulator(dev, "vss-supply",
+						  &uc_pdata->vss_supply);
+		if (ret)
+			return ret;
+	}
 
 	ret = regulator_get_value(uc_pdata->vss_supply);
 	if (ret < 0)
@@ -329,6 +351,30 @@ int adc_vss_value(struct udevice *dev, int *uV)
 	return 0;
 }
 
+int adc_raw_to_uV(struct udevice *dev, unsigned int raw, int *uV)
+{
+	unsigned int data_mask;
+	int ret, val, vref;
+	u64 raw64 = raw;
+
+	ret = adc_vdd_value(dev, &vref);
+	if (ret)
+		return ret;
+
+	if (!adc_vss_value(dev, &val))
+		vref -= val;
+
+	ret = adc_data_mask(dev, &data_mask);
+	if (ret)
+		return ret;
+
+	raw64 *= vref;
+	do_div(raw64, data_mask);
+	*uV = raw64;
+
+	return 0;
+}
+
 static int adc_vdd_platdata_set(struct udevice *dev)
 {
 	struct adc_uclass_platdata *uc_pdata = dev_get_uclass_platdata(dev);
diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index 6a8c7b7..79f834b 100644
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -12,13 +12,19 @@
 #include <syscon.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
+#include <asm/arch/stm32mp1_smc.h>
+#include <asm/arch/sys_proto.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/clock/stm32mp1-clksrc.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifndef CONFIG_STM32MP1_TRUSTED
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
 /* activate clock tree initialization in the driver */
 #define STM32MP1_CLOCK_TREE_INIT
 #endif
+#endif
 
 #define MAX_HSI_HZ		64000000
 
@@ -104,6 +110,7 @@
 #define RCC_MP_APB2ENSETR	0XA08
 #define RCC_MP_APB3ENSETR	0xA10
 #define RCC_MP_AHB2ENSETR	0xA18
+#define RCC_MP_AHB3ENSETR	0xA20
 #define RCC_MP_AHB4ENSETR	0xA28
 
 /* used for most of SELR register */
@@ -240,7 +247,6 @@ enum stm32mp1_parent_id {
 	_LSI,
 	_LSE,
 	_I2S_CKIN,
-	_USB_PHY_48,
 	NB_OSC,
 
 /* other parent source */
@@ -272,6 +278,7 @@ enum stm32mp1_parent_id {
 	_CK_MPU,
 	_CK_MCU,
 	_DSI_PHY,
+	_USB_PHY_48,
 	_PARENT_NB,
 	_UNKNOWN_ID = 0xff,
 };
@@ -377,6 +384,7 @@ struct stm32mp1_clk_gate {
 	u8 set_clr;
 	u8 sel;
 	u8 fixed;
+	bool secure;
 };
 
 struct stm32mp1_clk_sel {
@@ -421,6 +429,7 @@ struct stm32mp1_clk_priv {
 		.set_clr = 0,			\
 		.sel = (s),			\
 		.fixed = _UNKNOWN_ID,		\
+		.secure = 0,			\
 	}
 
 #define STM32MP1_CLK_F(off, b, idx, f)		\
@@ -431,6 +440,7 @@ struct stm32mp1_clk_priv {
 		.set_clr = 0,			\
 		.sel = _UNKNOWN_SEL,		\
 		.fixed = (f),			\
+		.secure = 0,			\
 	}
 
 #define STM32MP1_CLK_SET_CLR(off, b, idx, s)	\
@@ -441,6 +451,7 @@ struct stm32mp1_clk_priv {
 		.set_clr = 1,			\
 		.sel = (s),			\
 		.fixed = _UNKNOWN_ID,		\
+		.secure = 0,			\
 	}
 
 #define STM32MP1_CLK_SET_CLR_F(off, b, idx, f)	\
@@ -451,6 +462,18 @@ struct stm32mp1_clk_priv {
 		.set_clr = 1,			\
 		.sel = _UNKNOWN_SEL,		\
 		.fixed = (f),			\
+		.secure = 0,			\
+	}
+
+#define STM32MP1_CLK_SEC_SET_CLR(off, b, idx, s)	\
+	{					\
+		.offset = (off),		\
+		.bit = (b),			\
+		.index = (idx),			\
+		.set_clr = 1,			\
+		.sel = (s),			\
+		.fixed = _UNKNOWN_ID,		\
+		.secure = 1,			\
 	}
 
 #define STM32MP1_CLK_PARENT(idx, off, s, m, p)   \
@@ -526,14 +549,17 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
 
-	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
-	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
 
 	STM32MP1_CLK_SET_CLR_F(RCC_MP_AHB2ENSETR, 5, ADC12, _HCLK2),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 5, ADC12_K, _ADC12_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
 
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB3ENSETR, 11, HSEM, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB3ENSETR, 12, IPCC, _UNKNOWN_SEL),
+
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
@@ -546,7 +572,7 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
 
-	STM32MP1_CLK_SET_CLR(RCC_MP_AHB5ENSETR, 0, GPIOZ, _UNKNOWN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_AHB5ENSETR, 0, GPIOZ, _UNKNOWN_SEL),
 
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 7, ETHCK, _ETH_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 8, ETHTX, _UNKNOWN_SEL),
@@ -662,8 +688,8 @@ static const u8 stm32mp1_axi_div[8] = {
 	1, 2, 3, 4, 4, 4, 4, 4
 };
 
-#ifdef DEBUG
-static const char * const stm32mp1_clk_parent_name[_PARENT_NB] = {
+static const __maybe_unused
+char * const stm32mp1_clk_parent_name[_PARENT_NB] = {
 	[_HSI] = "HSI",
 	[_HSE] = "HSE",
 	[_CSI] = "CSI",
@@ -701,7 +727,8 @@ static const char * const stm32mp1_clk_parent_name[_PARENT_NB] = {
 	[_DSI_PHY] = "DSI_PHY_PLL",
 };
 
-static const char * const stm32mp1_clk_parent_sel_name[_PARENT_SEL_NB] = {
+static const __maybe_unused
+char * const stm32mp1_clk_parent_sel_name[_PARENT_SEL_NB] = {
 	[_I2C12_SEL] = "I2C12",
 	[_I2C35_SEL] = "I2C35",
 	[_I2C46_SEL] = "I2C46",
@@ -720,7 +747,6 @@ static const char * const stm32mp1_clk_parent_sel_name[_PARENT_SEL_NB] = {
 	[_DSI_SEL] = "DSI",
 	[_ADC12_SEL] = "ADC12",
 };
-#endif
 
 static const struct stm32mp1_clk_data stm32mp1_data = {
 	.gate = stm32mp1_clk_gate,
@@ -736,9 +762,6 @@ static ulong stm32mp1_clk_get_fixed(struct stm32mp1_clk_priv *priv, int idx)
 		return 0;
 	}
 
-	debug("%s: clk id %d = %x : %ld kHz\n", __func__, idx,
-	      (u32)priv->osc[idx], priv->osc[idx] / 1000);
-
 	return priv->osc[idx];
 }
 
@@ -839,8 +862,6 @@ static ulong  pll_get_fref_ck(struct stm32mp1_clk_priv *priv,
 	src = selr & RCC_SELR_SRC_MASK;
 
 	refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]);
-	debug("PLL%d : selr=%x refclk = %d kHz\n",
-	      pll_id, selr, (u32)(refclk / 1000));
 
 	return refclk;
 }
@@ -865,9 +886,6 @@ static ulong pll_get_fvco(struct stm32mp1_clk_priv *priv,
 	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
 	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
 
-	debug("PLL%d : cfgr1=%x fracr=%x DIVN=%d DIVM=%d\n",
-	      pll_id, cfgr1, fracr, divn, divm);
-
 	refclk = pll_get_fref_ck(priv, pll_id);
 
 	/* with FRACV :
@@ -884,7 +902,6 @@ static ulong pll_get_fvco(struct stm32mp1_clk_priv *priv,
 	} else {
 		fvco = (ulong)(refclk * (divn + 1) / (divm + 1));
 	}
-	debug("PLL%d : %s = %ld\n", pll_id, __func__, fvco);
 
 	return fvco;
 }
@@ -897,17 +914,12 @@ static ulong stm32mp1_read_pll_freq(struct stm32mp1_clk_priv *priv,
 	ulong dfout;
 	u32 cfgr2;
 
-	debug("%s(%d, %d)\n", __func__, pll_id, div_id);
 	if (div_id >= _DIV_NB)
 		return 0;
 
 	cfgr2 = readl(priv->base + pll[pll_id].pllxcfgr2);
 	divy = (cfgr2 >> RCC_PLLNCFGR2_SHIFT(div_id)) & RCC_PLLNCFGR2_DIVX_MASK;
-
-	debug("PLL%d : cfgr2=%x DIVY=%d\n", pll_id, cfgr2, divy);
-
 	dfout = pll_get_fvco(priv, pll_id) / (divy + 1);
-	debug("        => dfout = %d kHz\n", (u32)(dfout / 1000));
 
 	return dfout;
 }
@@ -918,8 +930,8 @@ static ulong stm32mp1_clk_get(struct stm32mp1_clk_priv *priv, int p)
 	ulong clock = 0;
 
 	switch (p) {
-	case _CK_MPU:
 	/* MPU sub system */
+	case _CK_MPU:
 		reg = readl(priv->base + RCC_MPCKSELR);
 		switch (reg & RCC_SELR_SRC_MASK) {
 		case RCC_MPCKSELR_HSI:
@@ -1076,7 +1088,7 @@ static ulong stm32mp1_clk_get(struct stm32mp1_clk_priv *priv, int p)
 		break;
 	/* other */
 	case _USB_PHY_48:
-		clock = stm32mp1_clk_get_fixed(priv, _USB_PHY_48);
+		clock = 48000000;
 		break;
 	case _DSI_PHY:
 	{
@@ -1114,7 +1126,13 @@ static int stm32mp1_clk_enable(struct clk *clk)
 		return i;
 
 	if (gate[i].set_clr)
-		writel(BIT(gate[i].bit), priv->base + gate[i].offset);
+#ifdef CONFIG_STM32MP1_TRUSTED
+		if (gate[i].secure)
+			stm32_smc_exec(STM32_SMC_RCC, STM32_SMC_REG_WRITE,
+				       gate[i].offset, BIT(gate[i].bit));
+		else
+#endif /* CONFIG_STM32MP1_TRUSTED */
+			writel(BIT(gate[i].bit), priv->base + gate[i].offset);
 	else
 		setbits_le32(priv->base + gate[i].offset, BIT(gate[i].bit));
 
@@ -1133,9 +1151,16 @@ static int stm32mp1_clk_disable(struct clk *clk)
 		return i;
 
 	if (gate[i].set_clr)
-		writel(BIT(gate[i].bit),
-		       priv->base + gate[i].offset
-		       + RCC_MP_ENCLRR_OFFSET);
+#ifdef CONFIG_STM32MP1_TRUSTED
+		if (gate[i].secure)
+			stm32_smc_exec(STM32_SMC_RCC, STM32_SMC_REG_WRITE,
+				       gate[i].offset + RCC_MP_ENCLRR_OFFSET,
+				       BIT(gate[i].bit));
+		else
+#endif /* CONFIG_STM32MP1_TRUSTED */
+			writel(BIT(gate[i].bit),
+			       priv->base + gate[i].offset
+			       + RCC_MP_ENCLRR_OFFSET);
 	else
 		clrbits_le32(priv->base + gate[i].offset, BIT(gate[i].bit));
 
@@ -1176,10 +1201,7 @@ static void stm32mp1_ls_osc_set(int enable, fdt_addr_t rcc, u32 offset,
 
 static void stm32mp1_hs_ocs_set(int enable, fdt_addr_t rcc, u32 mask_on)
 {
-	if (enable)
-		setbits_le32(rcc + RCC_OCENSETR, mask_on);
-	else
-		setbits_le32(rcc + RCC_OCENCLRR, mask_on);
+	writel(mask_on, rcc + (enable ? RCC_OCENSETR : RCC_OCENCLRR));
 }
 
 static int stm32mp1_osc_wait(int enable, fdt_addr_t rcc, u32 offset,
@@ -1250,20 +1272,20 @@ static void stm32mp1_lsi_set(fdt_addr_t rcc, int enable)
 static void stm32mp1_hse_enable(fdt_addr_t rcc, int bypass, int digbyp, int css)
 {
 	if (digbyp)
-		setbits_le32(rcc + RCC_OCENSETR, RCC_OCENR_DIGBYP);
+		writel(RCC_OCENR_DIGBYP, rcc + RCC_OCENSETR);
 	if (bypass || digbyp)
-		setbits_le32(rcc + RCC_OCENSETR, RCC_OCENR_HSEBYP);
+		writel(RCC_OCENR_HSEBYP, rcc + RCC_OCENSETR);
 
 	stm32mp1_hs_ocs_set(1, rcc, RCC_OCENR_HSEON);
 	stm32mp1_osc_wait(1, rcc, RCC_OCRDYR, RCC_OCRDYR_HSERDY);
 
 	if (css)
-		setbits_le32(rcc + RCC_OCENSETR, RCC_OCENR_HSECSSON);
+		writel(RCC_OCENR_HSECSSON, rcc + RCC_OCENSETR);
 }
 
 static void stm32mp1_csi_set(fdt_addr_t rcc, int enable)
 {
-	stm32mp1_ls_osc_set(enable, rcc, RCC_OCENSETR, RCC_OCENR_CSION);
+	stm32mp1_hs_ocs_set(enable, rcc, RCC_OCENR_CSION);
 	stm32mp1_osc_wait(enable, rcc, RCC_OCRDYR, RCC_OCRDYR_CSIRDY);
 }
 
@@ -1439,6 +1461,71 @@ static void pll_csg(struct stm32mp1_clk_priv *priv, int pll_id, u32 *csg)
 	writel(pllxcsg, priv->base + pll[pll_id].pllxcsgr);
 }
 
+static  __maybe_unused int pll_set_rate(struct udevice *dev,
+					int pll_id,
+					int div_id,
+					unsigned long clk_rate)
+{
+	struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
+	unsigned int pllcfg[PLLCFG_NB];
+	ofnode plloff;
+	char name[12];
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	enum stm32mp1_plltype type = pll[pll_id].plltype;
+	int divm, divn, divy;
+	int ret;
+	ulong fck_ref;
+	u32 fracv;
+	u64 value;
+
+	if (div_id > _DIV_NB)
+		return -EINVAL;
+
+	sprintf(name, "st,pll@%d", pll_id);
+	plloff = dev_read_subnode(dev, name);
+	if (!ofnode_valid(plloff))
+		return -FDT_ERR_NOTFOUND;
+
+	ret = ofnode_read_u32_array(plloff, "cfg",
+				    pllcfg, PLLCFG_NB);
+	if (ret < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	fck_ref = pll_get_fref_ck(priv, pll_id);
+
+	divm = pllcfg[PLLCFG_M];
+	/* select output divider = 0: for _DIV_P, 1:_DIV_Q 2:_DIV_R */
+	divy = pllcfg[PLLCFG_P + div_id];
+
+	/* For: PLL1 & PLL2 => VCO is * 2 but ck_pll_y is also / 2
+	 * So same final result than PLL2 et 4
+	 * with FRACV
+	 * Fck_pll_y = Fck_ref * ((DIVN + 1) + FRACV / 2^13)
+	 *             / (DIVy + 1) * (DIVM + 1)
+	 * value = (DIVN + 1) * 2^13 + FRACV / 2^13
+	 *       = Fck_pll_y (DIVy + 1) * (DIVM + 1) * 2^13 / Fck_ref
+	 */
+	value = ((u64)clk_rate * (divy + 1) * (divm + 1)) << 13;
+	value = lldiv(value, fck_ref);
+
+	divn = (value >> 13) - 1;
+	if (divn < DIVN_MIN ||
+	    divn > stm32mp1_pll[type].divn_max) {
+		pr_err("divn invalid = %d", divn);
+		return -EINVAL;
+	}
+	fracv = value - ((divn + 1) << 13);
+	pllcfg[PLLCFG_N] = divn;
+
+	/* reconfigure PLL */
+	pll_stop(priv, pll_id);
+	pll_config(priv, pll_id, pllcfg, fracv);
+	pll_start(priv, pll_id);
+	pll_output(priv, pll_id, pllcfg[PLLCFG_O]);
+
+	return 0;
+}
+
 static int set_clksrc(struct stm32mp1_clk_priv *priv, unsigned int clksrc)
 {
 	u32 address = priv->base + (clksrc >> 4);
@@ -1468,10 +1555,15 @@ static void stgen_config(struct stm32mp1_clk_priv *priv)
 	rate = stm32mp1_clk_get(priv, p);
 
 	if (cntfid0 != rate) {
+		u64 counter;
+
 		pr_debug("System Generic Counter (STGEN) update\n");
 		clrbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
-		writel(0x0, stgenc + STGENC_CNTCVL);
-		writel(0x0, stgenc + STGENC_CNTCVU);
+		counter = (u64)readl(stgenc + STGENC_CNTCVL);
+		counter |= ((u64)(readl(stgenc + STGENC_CNTCVU))) << 32;
+		counter = lldiv(counter * (u64)rate, cntfid0);
+		writel((u32)counter, stgenc + STGENC_CNTCVL);
+		writel((u32)(counter >> 32), stgenc + STGENC_CNTCVU);
 		writel(rate, stgenc + STGENC_CNTFID0);
 		setbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
 
@@ -1479,9 +1571,6 @@ static void stgen_config(struct stm32mp1_clk_priv *priv)
 
 		/* need to update gd->arch.timer_rate_hz with new frequency */
 		timer_init();
-		pr_debug("gd->arch.timer_rate_hz = %x\n",
-			 (u32)gd->arch.timer_rate_hz);
-		pr_debug("Tick = %x\n", (u32)(get_ticks()));
 	}
 }
 
@@ -1787,7 +1876,6 @@ static int pll_set_output_rate(struct udevice *dev,
 	if (div > 128)
 		div = 128;
 
-	debug("fvco = %ld, clk_rate = %ld, div=%d\n", fvco, clk_rate, div);
 	/* stop the requested output */
 	clrbits_le32(pllxcr, 0x1 << div_id << RCC_PLLNCR_DIVEN_SHIFT);
 	/* change divider */
@@ -1806,6 +1894,11 @@ static ulong stm32mp1_clk_set_rate(struct clk *clk, unsigned long clk_rate)
 	int p;
 
 	switch (clk->id) {
+#if defined(STM32MP1_CLOCK_TREE_INIT) && \
+	defined(CONFIG_STM32MP1_DDR_INTERACTIVE)
+	case DDRPHYC:
+		break;
+#endif
 	case LTDC_PX:
 	case DSI_PX:
 		break;
@@ -1815,10 +1908,27 @@ static ulong stm32mp1_clk_set_rate(struct clk *clk, unsigned long clk_rate)
 	}
 
 	p = stm32mp1_clk_get_parent(priv, clk->id);
+#ifdef DEBUG
+	debug("%s: parent = %d:%s\n", __func__, p, stm32mp1_clk_parent_name[p]);
+#endif
 	if (p < 0)
 		return -EINVAL;
 
 	switch (p) {
+#if defined(STM32MP1_CLOCK_TREE_INIT) && \
+	defined(CONFIG_STM32MP1_DDR_INTERACTIVE)
+	case _PLL2_R: /* DDRPHYC */
+	{
+		/* only for change DDR clock in interactive mode */
+		ulong result;
+
+		set_clksrc(priv, CLK_AXI_HSI);
+		result = pll_set_rate(clk->dev,  _PLL2, _DIV_R, clk_rate);
+		set_clksrc(priv, CLK_AXI_PLL2P);
+		return result;
+	}
+#endif
+
 	case _PLL4_Q:
 		/* for LTDC_PX and DSI_PX case */
 		return pll_set_output_rate(clk->dev, _PLL4, _DIV_Q, clk_rate);
@@ -1856,7 +1966,7 @@ static void stm32mp1_osc_init(struct udevice *dev)
 		[_HSE] = "clk-hse",
 		[_CSI] = "clk-csi",
 		[_I2S_CKIN] = "i2s_ckin",
-		[_USB_PHY_48] = "ck_usbo_48m"};
+	};
 
 	for (i = 0; i < NB_OSC; i++) {
 		stm32mp1_osc_clk_init(name[i], priv, i);
@@ -1864,6 +1974,54 @@ static void stm32mp1_osc_init(struct udevice *dev)
 	}
 }
 
+static void  __maybe_unused stm32mp1_clk_dump(struct stm32mp1_clk_priv *priv)
+{
+	char buf[32];
+	int i, s, p;
+
+	printf("Clocks:\n");
+	for (i = 0; i < _PARENT_NB; i++) {
+		printf("- %s : %s MHz\n",
+		       stm32mp1_clk_parent_name[i],
+		       strmhz(buf, stm32mp1_clk_get(priv, i)));
+	}
+	printf("Source Clocks:\n");
+	for (i = 0; i < _PARENT_SEL_NB; i++) {
+		p = (readl(priv->base + priv->data->sel[i].offset) >>
+		     priv->data->sel[i].src) & priv->data->sel[i].msk;
+		if (p < priv->data->sel[i].nb_parent) {
+			s = priv->data->sel[i].parent[p];
+			printf("- %s(%d) => parent %s(%d)\n",
+			       stm32mp1_clk_parent_sel_name[i], i,
+			       stm32mp1_clk_parent_name[s], s);
+		} else {
+			printf("- %s(%d) => parent index %d is invalid\n",
+			       stm32mp1_clk_parent_sel_name[i], i, p);
+		}
+	}
+}
+
+#ifdef CONFIG_CMD_CLK
+int soc_clk_dump(void)
+{
+	struct udevice *dev;
+	struct stm32mp1_clk_priv *priv;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_CLK,
+					  DM_GET_DRIVER(stm32mp1_clock),
+					  &dev);
+	if (ret)
+		return ret;
+
+	priv = dev_get_priv(dev);
+
+	stm32mp1_clk_dump(priv);
+
+	return 0;
+}
+#endif
+
 static int stm32mp1_clk_probe(struct udevice *dev)
 {
 	int result = 0;
@@ -1887,6 +2045,33 @@ static int stm32mp1_clk_probe(struct udevice *dev)
 		result = stm32mp1_clktree(dev);
 #endif
 
+#ifndef CONFIG_SPL_BUILD
+#if defined(DEBUG)
+	/* display debug information for probe after relocation */
+	if (gd->flags & GD_FLG_RELOC)
+		stm32mp1_clk_dump(priv);
+#endif
+
+#if defined(CONFIG_DISPLAY_CPUINFO)
+	if (gd->flags & GD_FLG_RELOC) {
+		char buf[32];
+
+		printf("Clocks:\n");
+		printf("- MPU : %s MHz\n",
+		       strmhz(buf, stm32mp1_clk_get(priv, _CK_MPU)));
+		printf("- MCU : %s MHz\n",
+		       strmhz(buf, stm32mp1_clk_get(priv, _CK_MCU)));
+		printf("- AXI : %s MHz\n",
+		       strmhz(buf, stm32mp1_clk_get(priv, _ACLK)));
+		printf("- PER : %s MHz\n",
+		       strmhz(buf, stm32mp1_clk_get(priv, _CK_PER)));
+		/* DDRPHYC father */
+		printf("- DDR : %s MHz\n",
+		       strmhz(buf, stm32mp1_clk_get(priv, _PLL2_R)));
+	}
+#endif /* CONFIG_DISPLAY_CPUINFO */
+#endif
+
 	return result;
 }
 
diff --git a/drivers/core/syscon-uclass.c b/drivers/core/syscon-uclass.c
index 303e166..85d82749 100644
--- a/drivers/core/syscon-uclass.c
+++ b/drivers/core/syscon-uclass.c
@@ -123,52 +123,49 @@ U_BOOT_DRIVER(generic_syscon) = {
  * The syscon node can be bound to another driver, but still works
  * as a syscon provider.
  */
-static LIST_HEAD(syscon_list);
-
-struct syscon {
-	ofnode node;
-	struct regmap *regmap;
-	struct list_head list;
-};
-
-static struct syscon *of_syscon_register(ofnode node)
+struct regmap *syscon_node_to_regmap(ofnode node)
 {
-	struct syscon *syscon;
+	struct udevice *dev, *parent;
+	ofnode ofnode = node;
 	int ret;
 
+	if (!uclass_get_device_by_ofnode(UCLASS_SYSCON, node, &dev))
+		return syscon_get_regmap(dev);
+
 	if (!ofnode_device_is_compatible(node, "syscon"))
 		return ERR_PTR(-EINVAL);
 
-	syscon = malloc(sizeof(*syscon));
-	if (!syscon)
-		return ERR_PTR(-ENOMEM);
+	if (device_find_global_by_ofnode(ofnode, &parent))
+		parent = dm_root();
 
-	ret = regmap_init_mem(node, &syscon->regmap);
-	if (ret) {
-		free(syscon);
+	/* force bound to syscon class */
+	ret = device_bind_driver_to_node(parent, "syscon",
+					 ofnode_get_name(node),
+					 node, &dev);
+	if (ret)
 		return ERR_PTR(ret);
-	}
 
-	list_add_tail(&syscon->list, &syscon_list);
+	ret = device_probe(dev);
+	if (ret)
+		return ERR_PTR(ret);
 
-	return syscon;
+	return syscon_get_regmap(dev);
 }
 
-struct regmap *syscon_node_to_regmap(ofnode node)
+struct regmap *syscon_phandle_to_regmap(struct udevice *parent,
+					const char *name)
 {
-	struct syscon *entry, *syscon = NULL;
-
-	list_for_each_entry(entry, &syscon_list, list)
-		if (ofnode_equal(entry->node, node)) {
-			syscon = entry;
-			break;
-		}
+	u32 phandle;
+	ofnode node;
+	int err;
 
-	if (!syscon)
-		syscon = of_syscon_register(node);
+	err = ofnode_read_u32(dev_ofnode(parent), name, &phandle);
+	if (err)
+		return ERR_PTR(err);
 
-	if (IS_ERR(syscon))
-		return ERR_CAST(syscon);
+	node = ofnode_get_by_phandle(phandle);
+	if (!ofnode_valid(node))
+		return ERR_PTR(-EINVAL);
 
-	return syscon->regmap;
+	return syscon_node_to_regmap(node);
 }
diff --git a/drivers/core/uclass.c b/drivers/core/uclass.c
index 3113d6a..a4452bd 100644
--- a/drivers/core/uclass.c
+++ b/drivers/core/uclass.c
@@ -562,6 +562,19 @@ int uclass_next_device(struct udevice **devp)
 	return uclass_get_device_tail(dev, ret, devp);
 }
 
+int uclass_next_device_err(struct udevice **devp)
+{
+	int ret;
+
+	ret = uclass_next_device(devp);
+	if (ret)
+		return ret;
+	else if (!*devp)
+		return -ENODEV;
+
+	return 0;
+}
+
 int uclass_first_device_check(enum uclass_id id, struct udevice **devp)
 {
 	int ret;
diff --git a/drivers/dfu/Kconfig b/drivers/dfu/Kconfig
index 4692736..edbe817 100644
--- a/drivers/dfu/Kconfig
+++ b/drivers/dfu/Kconfig
@@ -46,5 +46,10 @@ config DFU_SF
 	  This option enables using DFU to read and write to SPI flash based
 	  storage.
 
+config DFU_VIRT
+	bool "Virtual back end for DFU"
+	help
+	  This option enables using DFU to read and write to virtual partition.
+
 endif
 endmenu
diff --git a/drivers/dfu/Makefile b/drivers/dfu/Makefile
index 56f9b0c..830eab3 100644
--- a/drivers/dfu/Makefile
+++ b/drivers/dfu/Makefile
@@ -9,3 +9,4 @@ obj-$(CONFIG_DFU_NAND) += dfu_nand.o
 obj-$(CONFIG_DFU_RAM) += dfu_ram.o
 obj-$(CONFIG_DFU_SF) += dfu_sf.o
 obj-$(CONFIG_DFU_TFTP) += dfu_tftp.o
+obj-$(CONFIG_DFU_VIRT) += dfu_virt.o
diff --git a/drivers/dfu/dfu.c b/drivers/dfu/dfu.c
index 3189495..f7919b4 100644
--- a/drivers/dfu/dfu.c
+++ b/drivers/dfu/dfu.c
@@ -22,6 +22,22 @@ static int alt_num_cnt;
 static struct hash_algo *dfu_hash_algo;
 
 /*
+ * The purpose of the dfu_flush_callback() function is to
+ * provide callback for dfu user
+ */
+__weak void dfu_flush_callback(struct dfu_entity *dfu)
+{
+}
+
+/*
+ * The purpose of the dfu_flush_callback() function is to
+ * provide callback for dfu user
+ */
+__weak void dfu_initiated_callback(struct dfu_entity *dfu)
+{
+}
+
+/*
  * The purpose of the dfu_usb_get_reset() function is to
  * provide information if after USB_DETACH request
  * being sent the dfu-util performed reset of USB
@@ -82,6 +98,7 @@ done:
 
 static unsigned char *dfu_buf;
 static unsigned long dfu_buf_size;
+static enum dfu_device_type dfu_buf_device_type;
 
 unsigned char *dfu_free_buf(void)
 {
@@ -99,6 +116,10 @@ unsigned char *dfu_get_buf(struct dfu_entity *dfu)
 {
 	char *s;
 
+	/* manage several entity with several contraint */
+	if (dfu_buf && dfu->dev_type != dfu_buf_device_type)
+		dfu_free_buf();
+
 	if (dfu_buf != NULL)
 		return dfu_buf;
 
@@ -117,6 +138,7 @@ unsigned char *dfu_get_buf(struct dfu_entity *dfu)
 		printf("%s: Could not memalign 0x%lx bytes\n",
 		       __func__, dfu_buf_size);
 
+	dfu_buf_device_type = dfu->dev_type;
 	return dfu_buf;
 }
 
@@ -204,6 +226,7 @@ int dfu_transaction_initiate(struct dfu_entity *dfu, bool read)
 	}
 
 	dfu->inited = 1;
+	dfu_initiated_callback(dfu);
 
 	return 0;
 }
@@ -223,6 +246,8 @@ int dfu_flush(struct dfu_entity *dfu, void *buf, int size, int blk_seq_num)
 		printf("\nDFU complete %s: 0x%08x\n", dfu_hash_algo->name,
 		       dfu->crc);
 
+	dfu_flush_callback(dfu);
+
 	dfu_transaction_cleanup(dfu);
 
 	return ret;
@@ -337,6 +362,8 @@ static int dfu_read_buffer_fill(struct dfu_entity *dfu, void *buf, int size)
 				debug("%s: Read error!\n", __func__);
 				return ret;
 			}
+			if (dfu->b_left == 0)
+				break;
 			dfu->offset += dfu->b_left;
 			dfu->r_left -= dfu->b_left;
 
@@ -389,7 +416,8 @@ static int dfu_fill_entity(struct dfu_entity *dfu, char *s, int alt,
 {
 	char *st;
 
-	debug("%s: %s interface: %s dev: %s\n", __func__, s, interface, devstr);
+	debug("%s: %s interface: %s dev: %s\n",
+	      __func__, s, interface, devstr);
 	st = strsep(&s, " ");
 	strcpy(dfu->name, st);
 
@@ -410,11 +438,15 @@ static int dfu_fill_entity(struct dfu_entity *dfu, char *s, int alt,
 	} else if (strcmp(interface, "sf") == 0) {
 		if (dfu_fill_entity_sf(dfu, devstr, s))
 			return -1;
+	} else if (strcmp(interface, "virt") == 0) {
+		if (dfu_fill_entity_virt(dfu, devstr, s))
+			return -1;
 	} else {
 		printf("%s: Device %s not (yet) supported!\n",
 		       __func__,  interface);
 		return -1;
 	}
+
 	dfu_get_buf(dfu);
 
 	return 0;
@@ -438,13 +470,12 @@ void dfu_free_entities(void)
 	alt_num_cnt = 0;
 }
 
-int dfu_config_entities(char *env, char *interface, char *devstr)
+int dfu_alt_init(int num, struct dfu_entity **dfu)
 {
-	struct dfu_entity *dfu;
-	int i, ret;
 	char *s;
+	int ret;
 
-	dfu_alt_num = dfu_find_alt_num(env);
+	dfu_alt_num = num;
 	debug("%s: dfu_alt_num=%d\n", __func__, dfu_alt_num);
 
 	dfu_hash_algo = NULL;
@@ -455,21 +486,47 @@ int dfu_config_entities(char *env, char *interface, char *devstr)
 			pr_err("Hash algorithm %s not supported\n", s);
 	}
 
-	dfu = calloc(sizeof(*dfu), dfu_alt_num);
+	*dfu = calloc(sizeof(struct dfu_entity), dfu_alt_num);
 	if (!dfu)
 		return -1;
-	for (i = 0; i < dfu_alt_num; i++) {
+	return 0;
+}
 
+int dfu_alt_add(struct dfu_entity *dfu, char *interface, char *devstr, char *s)
+{
+	struct dfu_entity *p_dfu;
+	int ret;
+
+	if (alt_num_cnt >= dfu_alt_num)
+		return -1;
+
+	p_dfu = &dfu[alt_num_cnt];
+	ret = dfu_fill_entity(p_dfu, s, alt_num_cnt, interface, devstr);
+	if (ret)
+		return -1;
+
+	list_add_tail(&p_dfu->list, &dfu_list);
+	alt_num_cnt++;
+	return 0;
+}
+
+int dfu_config_entities(char *env, char *interface, char *devstr)
+{
+	struct dfu_entity *dfu;
+	int i, ret;
+	char *s;
+
+	ret = dfu_alt_init(dfu_find_alt_num(env), &dfu);
+	if (ret)
+		return -1;
+
+	for (i = 0; i < dfu_alt_num; i++) {
 		s = strsep(&env, ";");
-		ret = dfu_fill_entity(&dfu[i], s, alt_num_cnt, interface,
-				      devstr);
+		ret = dfu_alt_add(dfu, interface, devstr, s);
 		if (ret) {
 			/* We will free "dfu" in dfu_free_entities() */
 			return -1;
 		}
-
-		list_add_tail(&dfu[i].list, &dfu_list);
-		alt_num_cnt++;
 	}
 
 	return 0;
@@ -477,7 +534,8 @@ int dfu_config_entities(char *env, char *interface, char *devstr)
 
 const char *dfu_get_dev_type(enum dfu_device_type t)
 {
-	const char *dev_t[] = {NULL, "eMMC", "OneNAND", "NAND", "RAM", "SF" };
+	const char *dev_t[] = {NULL, "eMMC", "OneNAND", "NAND", "RAM", "SF",
+			       "VIRT"};
 	return dev_t[t];
 }
 
diff --git a/drivers/dfu/dfu_sf.c b/drivers/dfu/dfu_sf.c
index 066e767..65edba3 100644
--- a/drivers/dfu/dfu_sf.c
+++ b/drivers/dfu/dfu_sf.c
@@ -10,6 +10,8 @@
 #include <dfu.h>
 #include <spi.h>
 #include <spi_flash.h>
+#include <jffs2/load_kernel.h>
+#include <linux/mtd/mtd.h>
 
 static int dfu_get_medium_size_sf(struct dfu_entity *dfu, u64 *size)
 {
@@ -19,7 +21,7 @@ static int dfu_get_medium_size_sf(struct dfu_entity *dfu, u64 *size)
 }
 
 static int dfu_read_medium_sf(struct dfu_entity *dfu, u64 offset, void *buf,
-		long *len)
+			      long *len)
 {
 	return spi_flash_read(dfu->data.sf.dev, dfu->data.sf.start + offset,
 		*len, buf);
@@ -32,7 +34,7 @@ static u64 find_sector(struct dfu_entity *dfu, u64 start, u64 offset)
 }
 
 static int dfu_write_medium_sf(struct dfu_entity *dfu,
-		u64 offset, void *buf, long *len)
+			       u64 offset, void *buf, long *len)
 {
 	int ret;
 
@@ -52,11 +54,32 @@ static int dfu_write_medium_sf(struct dfu_entity *dfu,
 
 static int dfu_flush_medium_sf(struct dfu_entity *dfu)
 {
+	u64 off, length;
+
+	if (!dfu->data.sf.ubi)
+		return 0;
+
+	/* in case of ubi partition, erase rest of the partition */
+	off = find_sector(dfu, dfu->data.sf.start, dfu->offset);
+	/* last write ended with unaligned length jump to next */
+	if (off != dfu->data.sf.start + dfu->offset)
+		off += dfu->data.sf.dev->sector_size;
+	length = dfu->data.sf.start + dfu->data.sf.size - off;
+	if (length)
+		return spi_flash_erase(dfu->data.sf.dev, off, length);
+
 	return 0;
 }
 
 static unsigned int dfu_polltimeout_sf(struct dfu_entity *dfu)
 {
+	/*
+	 * Currently, Poll Timeout != 0 is only needed on nor
+	 * ubi partition, as the not used sectors need an erase
+	 */
+	if (dfu->data.sf.ubi)
+		return DFU_MANIFEST_POLL_TIMEOUT;
+
 	return DFU_DEFAULT_POLL_TIMEOUT;
 }
 
@@ -133,6 +156,33 @@ int dfu_fill_entity_sf(struct dfu_entity *dfu, char *devstr, char *s)
 		dfu->data.sf.start = simple_strtoul(s, &s, 16);
 		s++;
 		dfu->data.sf.size = simple_strtoul(s, &s, 16);
+	} else if ((!strcmp(st, "part")) || (!strcmp(st, "partubi"))) {
+		char mtd_id[32];
+		struct mtd_device *mtd_dev;
+		u8 part_num;
+		struct part_info *pi;
+		int ret, dev, part;
+
+		dfu->layout = DFU_RAW_ADDR;
+
+		dev = simple_strtoul(s, &s, 10);
+		s++;
+		part = simple_strtoul(s, &s, 10);
+
+		sprintf(mtd_id, "%s%d,%d", "nor", dev, part - 1);
+		printf("using id '%s'\n", mtd_id);
+
+		mtdparts_init();
+
+		ret = find_dev_and_part(mtd_id, &mtd_dev, &part_num, &pi);
+		if (ret != 0) {
+			printf("Could not locate '%s'\n", mtd_id);
+			return -1;
+		}
+		dfu->data.sf.start = pi->offset;
+		dfu->data.sf.size = pi->size;
+		if (!strcmp(st, "partubi"))
+			dfu->data.sf.ubi = 1;
 	} else {
 		printf("%s: Memory layout (%s) not supported!\n", __func__, st);
 		spi_flash_free(dfu->data.sf.dev);
diff --git a/drivers/dfu/dfu_virt.c b/drivers/dfu/dfu_virt.c
new file mode 100644
index 0000000..035d71f
--- /dev/null
+++ b/drivers/dfu/dfu_virt.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <malloc.h>
+#include <errno.h>
+#include <dfu.h>
+
+/* TODO : weak or function to implement ? */
+int __weak dfu_write_medium_virt(struct dfu_entity *dfu, u64 offset,
+				 void *buf, long *len)
+{
+	debug("%s: off=0x%llx, len=0x%x\n", __func__, offset, (u32)*len);
+	return 0;
+}
+
+int __weak dfu_get_medium_size_virt(struct dfu_entity *dfu, u64 *size)
+{
+	*size = 0;
+
+	return 0;
+}
+
+int __weak dfu_read_medium_virt(struct dfu_entity *dfu, u64 offset,
+				void *buf, long *len)
+{
+	debug("%s: off=0x%llx, len=0x%x\n", __func__, offset, (u32)*len);
+	*len = 0;
+	return 0;
+}
+
+int dfu_fill_entity_virt(struct dfu_entity *dfu, char *devstr, char *s)
+{
+	debug("%s: devstr = %s\n", __func__, devstr);
+	dfu->dev_type = DFU_DEV_VIRT;
+	dfu->layout = DFU_RAW_ADDR;
+	dfu->data.virt.dev_num = simple_strtoul(devstr, NULL, 10);
+	dfu->write_medium = dfu_write_medium_virt;
+	dfu->get_medium_size = dfu_get_medium_size_virt;
+	dfu->read_medium = dfu_read_medium_virt;
+
+	dfu->inited = 0;
+
+	return 0;
+}
diff --git a/drivers/gpio/stm32f7_gpio.c b/drivers/gpio/stm32f7_gpio.c
index 4c0786f..7fff64e 100644
--- a/drivers/gpio/stm32f7_gpio.c
+++ b/drivers/gpio/stm32f7_gpio.c
@@ -15,17 +15,45 @@
 #include <linux/errno.h>
 #include <linux/io.h>
 
-#define STM32_GPIOS_PER_BANK		16
 #define MODE_BITS(gpio_pin)		(gpio_pin * 2)
 #define MODE_BITS_MASK			3
 #define BSRR_BIT(gpio_pin, value)	BIT(gpio_pin + (value ? 0 : 16))
 
+/*
+ * convert gpio offset to gpio index due to potential gpio
+ * holes into gpio bank
+ */
+int stm32_offset_to_index(struct udevice *dev, unsigned int offset)
+{
+	struct stm32_gpio_priv *priv = dev_get_priv(dev);
+	int idx = 0;
+	int i;
+
+	for (i = 0; i < STM32_GPIOS_PER_BANK; i++) {
+		if (priv->gpio_range & BIT(i)) {
+			if (idx == offset)
+				return idx;
+			idx++;
+		}
+	}
+	/* shouldn't happen */
+	return -EINVAL;
+}
+
 static int stm32_gpio_direction_input(struct udevice *dev, unsigned offset)
 {
 	struct stm32_gpio_priv *priv = dev_get_priv(dev);
 	struct stm32_gpio_regs *regs = priv->regs;
-	int bits_index = MODE_BITS(offset);
-	int mask = MODE_BITS_MASK << bits_index;
+	int bits_index;
+	int mask;
+	int idx;
+
+	idx = stm32_offset_to_index(dev, offset);
+	if (idx < 0)
+		return idx;
+
+	bits_index = MODE_BITS(idx);
+	mask = MODE_BITS_MASK << bits_index;
 
 	clrsetbits_le32(&regs->moder, mask, STM32_GPIO_MODE_IN << bits_index);
 
@@ -37,12 +65,20 @@ static int stm32_gpio_direction_output(struct udevice *dev, unsigned offset,
 {
 	struct stm32_gpio_priv *priv = dev_get_priv(dev);
 	struct stm32_gpio_regs *regs = priv->regs;
-	int bits_index = MODE_BITS(offset);
-	int mask = MODE_BITS_MASK << bits_index;
+	int bits_index;
+	int mask;
+	int idx;
+
+	idx = stm32_offset_to_index(dev, offset);
+	if (idx < 0)
+		return idx;
+
+	bits_index = MODE_BITS(idx);
+	mask = MODE_BITS_MASK << bits_index;
 
 	clrsetbits_le32(&regs->moder, mask, STM32_GPIO_MODE_OUT << bits_index);
 
-	writel(BSRR_BIT(offset, value), &regs->bsrr);
+	writel(BSRR_BIT(idx, value), &regs->bsrr);
 
 	return 0;
 }
@@ -51,33 +87,75 @@ static int stm32_gpio_get_value(struct udevice *dev, unsigned offset)
 {
 	struct stm32_gpio_priv *priv = dev_get_priv(dev);
 	struct stm32_gpio_regs *regs = priv->regs;
+	int idx;
 
-	return readl(&regs->idr) & BIT(offset) ? 1 : 0;
+	idx = stm32_offset_to_index(dev, offset);
+	if (idx < 0)
+		return idx;
+
+	return readl(&regs->idr) & BIT(idx) ? 1 : 0;
 }
 
 static int stm32_gpio_set_value(struct udevice *dev, unsigned offset, int value)
 {
 	struct stm32_gpio_priv *priv = dev_get_priv(dev);
 	struct stm32_gpio_regs *regs = priv->regs;
+	int idx;
+
+	idx = stm32_offset_to_index(dev, offset);
+	if (idx < 0)
+		return idx;
 
-	writel(BSRR_BIT(offset, value), &regs->bsrr);
+	writel(BSRR_BIT(idx, value), &regs->bsrr);
 
 	return 0;
 }
 
+static int stm32_gpio_get_function(struct udevice *dev, unsigned int offset)
+{
+	struct stm32_gpio_priv *priv = dev_get_priv(dev);
+	struct stm32_gpio_regs *regs = priv->regs;
+	int bits_index;
+	int mask;
+	int idx;
+	u32 mode;
+
+	idx = stm32_offset_to_index(dev, offset);
+	if (idx < 0)
+		return idx;
+
+	bits_index = MODE_BITS(idx);
+	mask = MODE_BITS_MASK << bits_index;
+
+	mode = (readl(&regs->moder) & mask) >> bits_index;
+	if (mode == STM32_GPIO_MODE_OUT)
+		return GPIOF_OUTPUT;
+	if (mode == STM32_GPIO_MODE_IN)
+		return GPIOF_INPUT;
+	if (mode == STM32_GPIO_MODE_AN)
+		return GPIOF_UNUSED;
+
+	return GPIOF_FUNC;
+}
+
 static const struct dm_gpio_ops gpio_stm32_ops = {
 	.direction_input	= stm32_gpio_direction_input,
 	.direction_output	= stm32_gpio_direction_output,
 	.get_value		= stm32_gpio_get_value,
 	.set_value		= stm32_gpio_set_value,
+	.get_function		= stm32_gpio_get_function,
 };
 
 static int gpio_stm32_probe(struct udevice *dev)
 {
 	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct stm32_gpio_priv *priv = dev_get_priv(dev);
+	struct ofnode_phandle_args args;
+	struct clk clk;
 	fdt_addr_t addr;
 	const char *name;
+	int ret;
+	int i;
 
 	addr = dev_read_addr(dev);
 	if (addr == FDT_ADDR_T_NONE)
@@ -88,14 +166,25 @@ static int gpio_stm32_probe(struct udevice *dev)
 	if (!name)
 		return -EINVAL;
 	uc_priv->bank_name = name;
-	uc_priv->gpio_count = dev_read_u32_default(dev, "ngpios",
-						   STM32_GPIOS_PER_BANK);
-	debug("%s, addr = 0x%p, bank_name = %s\n", __func__, (u32 *)priv->regs,
-	      uc_priv->bank_name);
 
-#ifdef CONFIG_CLK
-	struct clk clk;
-	int ret;
+	i = 0;
+	ret = dev_read_phandle_with_args(dev, "gpio-ranges",
+					 NULL, 3, i, &args);
+
+	while (ret != -ENOENT) {
+		priv->gpio_range |= GENMASK(args.args[2] + args.args[0] - 1,
+				    args.args[0]);
+
+		uc_priv->gpio_count += args.args[2];
+
+		ret = dev_read_phandle_with_args(dev, "gpio-ranges", NULL, 3,
+						 ++i, &args);
+	}
+
+	dev_dbg(dev, "addr = 0x%p bank_name = %s gpio_count = %d gpio_range = 0x%x\n",
+		(u32 *)priv->regs, uc_priv->bank_name, uc_priv->gpio_count,
+		priv->gpio_range);
+
 	ret = clk_get_by_index(dev, 0, &clk);
 	if (ret < 0)
 		return ret;
@@ -107,7 +196,6 @@ static int gpio_stm32_probe(struct udevice *dev)
 		return ret;
 	}
 	debug("clock enabled for device %s\n", dev->name);
-#endif
 
 	return 0;
 }
diff --git a/drivers/hwspinlock/Kconfig b/drivers/hwspinlock/Kconfig
new file mode 100644
index 0000000..96d4f5d
--- /dev/null
+++ b/drivers/hwspinlock/Kconfig
@@ -0,0 +1,24 @@
+menu "Hardware Spinlock Support"
+
+config DM_HWSPINLOCK
+	bool "Enable U-Boot hardware spinlock support"
+	help
+	  This option enables U-Boot hardware spinlock support
+
+config HWSPINLOCK_SANDBOX
+	bool "Enable Hardware Spinlock support for Sandbox"
+	depends on SANDBOX && DM_HWSPINLOCK
+	help
+	  Enable hardware spinlock support in Sandbox. This is a dummy device that
+	  can be probed and support all the methods of HWSPINLOCK, but does not
+	  really do anything.
+
+config HWSPINLOCK_STM32
+	bool "Enable Hardware Spinlock support for STM32"
+	depends on ARCH_STM32MP && DM_HWSPINLOCK
+	help
+	  Enable hardware spinlock support in STM32MP. Hardware spinlocks are
+	  hardware mutex which provide a synchronisation mechanism for the
+	  various processors on the SoC.
+
+endmenu
diff --git a/drivers/hwspinlock/Makefile b/drivers/hwspinlock/Makefile
new file mode 100644
index 0000000..289b12a
--- /dev/null
+++ b/drivers/hwspinlock/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+#
+# Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+
+obj-$(CONFIG_DM_HWSPINLOCK) += hwspinlock-uclass.o
+obj-$(CONFIG_HWSPINLOCK_SANDBOX) += sandbox_hwspinlock.o
+obj-$(CONFIG_HWSPINLOCK_STM32) += stm32_hwspinlock.o
diff --git a/drivers/hwspinlock/hwspinlock-uclass.c b/drivers/hwspinlock/hwspinlock-uclass.c
new file mode 100644
index 0000000..195f079
--- /dev/null
+++ b/drivers/hwspinlock/hwspinlock-uclass.c
@@ -0,0 +1,144 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <hwspinlock.h>
+#include <dm/device-internal.h>
+
+static inline const struct hwspinlock_ops *
+hwspinlock_dev_ops(struct udevice *dev)
+{
+	return (const struct hwspinlock_ops *)dev->driver->ops;
+}
+
+static int hwspinlock_of_xlate_default(struct hwspinlock *hws,
+				       struct ofnode_phandle_args *args)
+{
+	if (args->args_count > 1) {
+		debug("Invaild args_count: %d\n", args->args_count);
+		return -EINVAL;
+	}
+
+	if (args->args_count)
+		hws->id = args->args[0];
+	else
+		hws->id = 0;
+
+	return 0;
+}
+
+int hwspinlock_get_by_index(struct udevice *dev, int index,
+			    struct hwspinlock *hws)
+{
+	int ret;
+	struct ofnode_phandle_args args;
+	struct udevice *dev_hws;
+	const struct hwspinlock_ops *ops;
+
+	assert(hws);
+	hws->dev = NULL;
+
+	ret = dev_read_phandle_with_args(dev, "hwlocks", "#hwlock-cells", 1,
+					 index, &args);
+	if (ret) {
+		dev_dbg(dev, "%s: dev_read_phandle_with_args: err=%d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	ret = uclass_get_device_by_ofnode(UCLASS_HWSPINLOCK,
+					  args.node, &dev_hws);
+	if (ret) {
+		dev_dbg(dev,
+			"%s: uclass_get_device_by_of_offset failed: err=%d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	hws->dev = dev_hws;
+
+	ops = hwspinlock_dev_ops(dev_hws);
+
+	if (ops->of_xlate)
+		ret = ops->of_xlate(hws, &args);
+	else
+		ret = hwspinlock_of_xlate_default(hws, &args);
+	if (ret)
+		dev_dbg(dev, "of_xlate() failed: %d\n", ret);
+
+	return ret;
+}
+
+int hwspinlock_lock_timeout(struct hwspinlock *hws, unsigned int timeout)
+{
+	const struct hwspinlock_ops *ops;
+	ulong start;
+	int ret;
+
+	assert(hws);
+
+	if (!hws->dev)
+		return -EINVAL;
+
+	ops = hwspinlock_dev_ops(hws->dev);
+	if (!ops->lock)
+		return -ENOSYS;
+
+	start = get_timer(0);
+	do {
+		ret = ops->lock(hws->dev, hws->id);
+		if (!ret)
+			return ret;
+
+		if (ops->relax)
+			ops->relax(hws->dev);
+	} while (get_timer(start) < timeout);
+
+	return -ETIMEDOUT;
+}
+
+int hwspinlock_unlock(struct hwspinlock *hws)
+{
+	const struct hwspinlock_ops *ops;
+
+	assert(hws);
+
+	if (!hws->dev)
+		return -EINVAL;
+
+	ops = hwspinlock_dev_ops(hws->dev);
+	if (!ops->unlock)
+		return -ENOSYS;
+
+	return ops->unlock(hws->dev, hws->id);
+}
+
+static int hwspinlock_post_bind(struct udevice *dev)
+{
+#if defined(CONFIG_NEEDS_MANUAL_RELOC)
+	struct hwspinlock_ops *ops = device_get_ops(dev);
+	static int reloc_done;
+
+	if (!reloc_done) {
+		if (ops->lock)
+			ops->lock += gd->reloc_off;
+		if (ops->unlock)
+			ops->unlock += gd->reloc_off;
+		if (ops->relax)
+			ops->relax += gd->reloc_off;
+
+		reloc_done++;
+	}
+#endif
+	return 0;
+}
+
+UCLASS_DRIVER(hwspinlock) = {
+	.id		= UCLASS_HWSPINLOCK,
+	.name		= "hwspinlock",
+	.post_bind	= hwspinlock_post_bind,
+};
diff --git a/drivers/hwspinlock/sandbox_hwspinlock.c b/drivers/hwspinlock/sandbox_hwspinlock.c
new file mode 100644
index 0000000..be920f5
--- /dev/null
+++ b/drivers/hwspinlock/sandbox_hwspinlock.c
@@ -0,0 +1,56 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <hwspinlock.h>
+#include <asm/state.h>
+
+static int sandbox_lock(struct udevice *dev, int index)
+{
+	struct sandbox_state *state = state_get_current();
+
+	if (index != 0)
+		return -1;
+
+	if (state->hwspinlock)
+		return -1;
+
+	state->hwspinlock = true;
+
+	return 0;
+}
+
+static int sandbox_unlock(struct udevice *dev, int index)
+{
+	struct sandbox_state *state = state_get_current();
+
+	if (index != 0)
+		return -1;
+
+	if (!state->hwspinlock)
+		return -1;
+
+	state->hwspinlock = false;
+
+	return 0;
+}
+
+static const struct hwspinlock_ops sandbox_hwspinlock_ops = {
+	.lock = sandbox_lock,
+	.unlock = sandbox_unlock,
+};
+
+static const struct udevice_id sandbox_hwspinlock_ids[] = {
+	{ .compatible = "sandbox,hwspinlock" },
+	{}
+};
+
+U_BOOT_DRIVER(hwspinlock_sandbox) = {
+	.name = "hwspinlock_sandbox",
+	.id = UCLASS_HWSPINLOCK,
+	.of_match = sandbox_hwspinlock_ids,
+	.ops = &sandbox_hwspinlock_ops,
+};
diff --git a/drivers/hwspinlock/stm32_hwspinlock.c b/drivers/hwspinlock/stm32_hwspinlock.c
new file mode 100644
index 0000000..b8f3b16
--- /dev/null
+++ b/drivers/hwspinlock/stm32_hwspinlock.c
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <linux/libfdt.h>
+#include <hwspinlock.h>
+#include <asm/io.h>
+
+#define STM32_MUTEX_COREID	BIT(8)
+#define STM32_MUTEX_LOCK_BIT	BIT(31)
+#define STM32_MUTEX_NUM_LOCKS	32
+
+static int stm32mp1_lock(struct udevice *dev, int index)
+{
+	fdt_addr_t *base = dev_get_priv(dev);
+	u32 status;
+
+	if (index >= STM32_MUTEX_NUM_LOCKS)
+		return -EINVAL;
+
+	status = readl(*base + index * sizeof(u32));
+	if (status == (STM32_MUTEX_LOCK_BIT | STM32_MUTEX_COREID))
+		return -EBUSY;
+
+	writel(STM32_MUTEX_LOCK_BIT | STM32_MUTEX_COREID,
+	       *base + index * sizeof(u32));
+
+	status = readl(*base + index * sizeof(u32));
+	if (status != (STM32_MUTEX_LOCK_BIT | STM32_MUTEX_COREID))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int stm32mp1_unlock(struct udevice *dev, int index)
+{
+	fdt_addr_t *base = dev_get_priv(dev);
+
+	if (index >= STM32_MUTEX_NUM_LOCKS)
+		return -EINVAL;
+
+	writel(STM32_MUTEX_COREID, *base + index * sizeof(u32));
+
+	return 0;
+}
+
+static int stm32mp1_hwspinlock_probe(struct udevice *dev)
+{
+	fdt_addr_t *base = dev_get_priv(dev);
+	struct clk clk;
+	int ret;
+
+	*base = dev_read_addr(dev);
+	if (*base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	ret = clk_get_by_index(dev, 0, &clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&clk);
+	if (ret)
+		clk_free(&clk);
+
+	return ret;
+}
+
+static const struct hwspinlock_ops stm32mp1_hwspinlock_ops = {
+	.lock = stm32mp1_lock,
+	.unlock = stm32mp1_unlock,
+};
+
+static const struct udevice_id stm32mp1_hwspinlock_ids[] = {
+	{ .compatible = "st,stm32-hwspinlock" },
+	{}
+};
+
+U_BOOT_DRIVER(hwspinlock_stm32mp1) = {
+	.name = "hwspinlock_stm32mp1",
+	.id = UCLASS_HWSPINLOCK,
+	.of_match = stm32mp1_hwspinlock_ids,
+	.ops = &stm32mp1_hwspinlock_ops,
+	.probe = stm32mp1_hwspinlock_probe,
+	.priv_auto_alloc_size = sizeof(fdt_addr_t),
+};
diff --git a/drivers/i2c/stm32f7_i2c.c b/drivers/i2c/stm32f7_i2c.c
index 36ec610..50c4fd0 100644
--- a/drivers/i2c/stm32f7_i2c.c
+++ b/drivers/i2c/stm32f7_i2c.c
@@ -58,7 +58,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 \
@@ -197,7 +197,7 @@ struct stm32_i2c_priv {
 	int speed;
 };
 
-static struct stm32_i2c_spec i2c_specs[] = {
+static const struct stm32_i2c_spec i2c_specs[] = {
 	[STM32_I2C_SPEED_STANDARD] = {
 		.rate = STANDARD_RATE,
 		.rate_min = 8000,
@@ -236,7 +236,7 @@ static struct stm32_i2c_spec i2c_specs[] = {
 	},
 };
 
-static struct stm32_i2c_setup stm32f7_setup = {
+static const 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,
@@ -255,7 +255,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);
@@ -299,7 +299,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);
@@ -317,7 +317,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);
@@ -392,7 +392,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;
@@ -465,7 +465,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;
@@ -500,7 +500,7 @@ 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 = i2c_specs[setup->speed].hddat_min + setup->fall_time -
 		     af_delay_min - (setup->dnf + 3) * i2cclk;
 
 	sdadel_max = i2c_specs[setup->speed].vddat_max - setup->rise_time -
@@ -540,8 +540,12 @@ 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;
 		}
 	}
 
@@ -594,6 +598,7 @@ 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) ||
 			    (i2cclk >=
 			     ((tscl_l - af_delay_min - dnf_delay) / 4))) {
@@ -634,8 +639,8 @@ static int stm32_i2c_choose_solution(struct stm32_i2c_setup *setup,
 }
 
 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_setup *setup,
+				    struct stm32_i2c_timings *output)
 {
 	struct stm32_i2c_timings *v, *_v;
 	struct list_head solutions;
@@ -643,28 +648,28 @@ static int stm32_i2c_compute_timing(struct stm32_i2c_priv *i2c_priv,
 
 	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);
+		       setup->speed, STM32_I2C_SPEED_END - 1);
 		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);
+		       __func__,
+		       setup->rise_time, i2c_specs[setup->speed].rise_max,
+		       setup->fall_time, i2c_specs[setup->speed].fall_max);
 		return -EINVAL;
 	}
 
 	if (setup->dnf > STM32_I2C_DNF_MAX) {
 		pr_err("%s: DNF out of bound %d/%d\n", __func__,
-		      setup->dnf, STM32_I2C_DNF_MAX);
+		       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);
+		       setup->speed_freq, i2c_specs[setup->speed].rate);
 		return -EINVAL;
 	}
 
@@ -693,7 +698,7 @@ exit:
 }
 
 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;
 	int ret = 0;
diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
index 2836ee4..82a8a87 100644
--- a/drivers/mailbox/Kconfig
+++ b/drivers/mailbox/Kconfig
@@ -24,6 +24,13 @@ config TEGRA_HSP
 	  This enables support for the NVIDIA Tegra HSP Hw module, which
 	  implements doorbells, mailboxes, semaphores, and shared interrupts.
 
+config STM32_IPCC
+	bool "Enable STM32 IPCC controller support"
+	depends on DM_MAILBOX && ARCH_STM32MP
+	help
+	  This enables support for the STM32MP IPCC Hw module, which
+	  implements doorbells and shared interrupts between 2 processors.
+
 config K3_SEC_PROXY
 	bool "Texas Instruments K3 Secure Proxy Driver"
 	depends on DM_MAILBOX && ARCH_K3
diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
index cd23769..a753cc4 100644
--- a/drivers/mailbox/Makefile
+++ b/drivers/mailbox/Makefile
@@ -6,5 +6,6 @@
 obj-$(CONFIG_$(SPL_)DM_MAILBOX) += mailbox-uclass.o
 obj-$(CONFIG_SANDBOX_MBOX) += sandbox-mbox.o
 obj-$(CONFIG_SANDBOX_MBOX) += sandbox-mbox-test.o
+obj-$(CONFIG_STM32_IPCC) += stm32-ipcc.o
 obj-$(CONFIG_TEGRA_HSP) += tegra-hsp.o
 obj-$(CONFIG_K3_SEC_PROXY) += k3-sec-proxy.o
diff --git a/drivers/mailbox/stm32-ipcc.c b/drivers/mailbox/stm32-ipcc.c
new file mode 100644
index 0000000..dfcea91
--- /dev/null
+++ b/drivers/mailbox/stm32-ipcc.c
@@ -0,0 +1,187 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <mailbox-uclass.h>
+#include <asm/io.h>
+
+/*
+ * IPCC has one set of registers per CPU
+ * IPCC_PROC_OFFST allows to define cpu registers set base address
+ * according to assigned cpu_id.
+ * In register name, X (Y) means either C1 (C2) or C2 (C1) depending
+ * of cpu offset.
+ */
+
+#define IPCC_PROC_OFFST		0x010
+
+#define IPCC_XCR		0x000
+#define XCR_RXOIE		BIT(0)
+#define XCR_TXOIE		BIT(16)
+
+#define IPCC_XMR		0x004
+#define IPCC_XSCR		0x008
+#define IPCC_XTOYSR		0x00c
+
+#define IPCC_HWCFGR		0x3f0
+#define IPCFGR_CHAN_MASK	GENMASK(7, 0)
+
+#define IPCC_VER		0x3f4
+#define VER_MINREV_MASK		GENMASK(3, 0)
+#define VER_MAJREV_MASK		GENMASK(7, 4)
+
+#define RX_BIT_MASK		GENMASK(15, 0)
+#define RX_BIT_CHAN(chan)	BIT(chan)
+#define TX_BIT_SHIFT		16
+#define TX_BIT_MASK		GENMASK(31, 16)
+#define TX_BIT_CHAN(chan)	BIT(TX_BIT_SHIFT + (chan))
+
+#define STM32_MAX_PROCS		2
+
+enum {
+	IPCC_IRQ_RX,
+	IPCC_IRQ_TX,
+	IPCC_IRQ_NUM,
+};
+
+struct stm32_ipcc {
+	void __iomem *reg_base;
+	void __iomem *reg_proc;
+	struct clk clk;
+	u32 proc_id;
+	u32 n_chans;
+};
+
+static int stm32_ipcc_request(struct mbox_chan *chan)
+{
+	struct stm32_ipcc *ipcc = dev_get_priv(chan->dev);
+
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	if (chan->id >= ipcc->n_chans) {
+		debug("%s failed to request channel: %ld\n",
+		      __func__, chan->id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int stm32_ipcc_free(struct mbox_chan *chan)
+{
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	return 0;
+}
+
+static int stm32_ipcc_send(struct mbox_chan *chan, const void *data)
+{
+	struct stm32_ipcc *ipcc = dev_get_priv(chan->dev);
+
+	debug("%s(chan=%p, data=%p)\n", __func__, chan, data);
+
+	/* set channel n occupied */
+	setbits_le32(ipcc->reg_proc + IPCC_XSCR, TX_BIT_CHAN(chan->id));
+
+	return 0;
+}
+
+static int stm32_ipcc_recv(struct mbox_chan *chan, void *data)
+{
+	struct stm32_ipcc *ipcc = dev_get_priv(chan->dev);
+	u32 val;
+	int proc_offset;
+
+	debug("%s(chan=%p, data=%p)\n", __func__, chan, data);
+
+	/* read 'channel occupied' status from other proc */
+	proc_offset = ipcc->proc_id ? -IPCC_PROC_OFFST : IPCC_PROC_OFFST;
+	val = readl(ipcc->reg_proc + proc_offset + IPCC_XTOYSR);
+
+	if (!(val & BIT(chan->id)))
+		return -ENODATA;
+
+	setbits_le32(ipcc->reg_proc + IPCC_XSCR, RX_BIT_CHAN(chan->id));
+
+	return 0;
+}
+
+static int stm32_ipcc_probe(struct udevice *dev)
+{
+	struct stm32_ipcc *ipcc = dev_get_priv(dev);
+	fdt_addr_t addr;
+	const fdt32_t *cell;
+	int len, ret;
+
+	debug("%s(dev=%p)\n", __func__, dev);
+
+	addr = dev_read_addr(dev);
+	if (addr == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	ipcc->reg_base = (void __iomem *)addr;
+
+	/* proc_id */
+	cell = dev_read_prop(dev, "st,proc_id", &len);
+	if (len < sizeof(fdt32_t)) {
+		dev_dbg(dev, "Missing st,proc_id\n");
+		return -EINVAL;
+	}
+
+	ipcc->proc_id = fdtdec_get_number(cell, 1);
+
+	if (ipcc->proc_id >= STM32_MAX_PROCS) {
+		dev_err(dev, "Invalid proc_id (%d)\n", ipcc->proc_id);
+		return -EINVAL;
+	}
+
+	ipcc->reg_proc = ipcc->reg_base + ipcc->proc_id * IPCC_PROC_OFFST;
+
+	ret = clk_get_by_index(dev, 0, &ipcc->clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&ipcc->clk);
+	if (ret)
+		goto clk_free;
+
+	/* get channe number */
+	ipcc->n_chans = readl(ipcc->reg_base + IPCC_HWCFGR);
+	ipcc->n_chans &= IPCFGR_CHAN_MASK;
+
+	/* mask and enable rx/tx irq */
+	setbits_le32(ipcc->reg_proc + IPCC_XMR, RX_BIT_MASK | TX_BIT_MASK);
+	setbits_le32(ipcc->reg_proc + IPCC_XCR, XCR_RXOIE | XCR_TXOIE);
+
+	return 0;
+
+clk_free:
+	clk_free(&ipcc->clk);
+
+	return ret;
+}
+
+static const struct udevice_id stm32_ipcc_ids[] = {
+	{ .compatible = "st,stm32mp1-ipcc" },
+	{ }
+};
+
+struct mbox_ops stm32_ipcc_mbox_ops = {
+	.request = stm32_ipcc_request,
+	.free = stm32_ipcc_free,
+	.send = stm32_ipcc_send,
+	.recv = stm32_ipcc_recv,
+};
+
+U_BOOT_DRIVER(stm32_ipcc) = {
+	.name = "stm32_ipcc",
+	.id = UCLASS_MAILBOX,
+	.of_match = stm32_ipcc_ids,
+	.probe = stm32_ipcc_probe,
+	.priv_auto_alloc_size = sizeof(struct stm32_ipcc),
+	.ops = &stm32_ipcc_mbox_ops,
+};
diff --git a/drivers/misc/stm32mp_fuse.c b/drivers/misc/stm32mp_fuse.c
index 2d66135..842871f 100644
--- a/drivers/misc/stm32mp_fuse.c
+++ b/drivers/misc/stm32mp_fuse.c
@@ -9,8 +9,10 @@
 #include <errno.h>
 #include <dm/device.h>
 #include <dm/uclass.h>
+#include <power/stpmic1.h>
 
 #define STM32MP_OTP_BANK	0
+#define STM32MP_NVM_BANK	1
 
 /*
  * The 'fuse' command API
@@ -31,6 +33,13 @@ int fuse_read(u32 bank, u32 word, u32 *val)
 				val, 4);
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		*val = 0;
+		ret = stpmic1_shadow_read_byte(word, (u8 *)val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -56,6 +65,12 @@ int fuse_prog(u32 bank, u32 word, u32 val)
 				 &val, 4);
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		ret = stpmic1_nvm_write_byte(word, (u8 *)&val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -80,6 +95,13 @@ int fuse_sense(u32 bank, u32 word, u32 *val)
 		ret = misc_read(dev, word * 4 + STM32_BSEC_OTP_OFFSET, val, 4);
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		*val = 0;
+		ret = stpmic1_nvm_read_byte(word, (u8 *)val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -105,6 +127,12 @@ int fuse_override(u32 bank, u32 word, u32 val)
 				 &val, 4);
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		ret = stpmic1_shadow_write_byte(word, (u8 *)&val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n",
 		       __func__, bank);
diff --git a/drivers/mmc/mmc_write.c b/drivers/mmc/mmc_write.c
index b8acc33..1a3f561 100644
--- a/drivers/mmc/mmc_write.c
+++ b/drivers/mmc/mmc_write.c
@@ -79,7 +79,7 @@ ulong mmc_berase(struct blk_desc *block_dev, lbaint_t start, lbaint_t blkcnt)
 	u32 start_rem, blkcnt_rem;
 	struct mmc *mmc = find_mmc_device(dev_num);
 	lbaint_t blk = 0, blk_r = 0;
-	int timeout = 1000;
+	int timeout = 2000;
 
 	if (!mmc)
 		return -1;
diff --git a/drivers/mmc/stm32_sdmmc2.c b/drivers/mmc/stm32_sdmmc2.c
index a36612d..ed31ca1 100644
--- a/drivers/mmc/stm32_sdmmc2.c
+++ b/drivers/mmc/stm32_sdmmc2.c
@@ -190,6 +190,7 @@ struct stm32_sdmmc2_ctx {
 #define SDMMC_IDMACTRL_IDMAEN		BIT(0)
 
 #define SDMMC_CMD_TIMEOUT		0xFFFFFFFF
+#define SDMMC_BUSYD0END_TIMEOUT_US	1000000
 
 static void stm32_sdmmc2_start_data(struct stm32_sdmmc2_priv *priv,
 				    struct mmc_data *data,
@@ -209,9 +210,6 @@ static void stm32_sdmmc2_start_data(struct stm32_sdmmc2_priv *priv,
 		idmabase0 = (u32)data->src;
 	}
 
-	/* Set the SDMMC Data TimeOut value */
-	writel(SDMMC_CMD_TIMEOUT, priv->base + SDMMC_DTIMER);
-
 	/* Set the SDMMC DataLength value */
 	writel(ctx->data_length, priv->base + SDMMC_DLEN);
 
@@ -236,8 +234,11 @@ static void stm32_sdmmc2_start_data(struct stm32_sdmmc2_priv *priv,
 }
 
 static void stm32_sdmmc2_start_cmd(struct stm32_sdmmc2_priv *priv,
-				   struct mmc_cmd *cmd, u32 cmd_param)
+				   struct mmc_cmd *cmd, u32 cmd_param,
+				   struct stm32_sdmmc2_ctx *ctx)
 {
+	u32 timeout = 0;
+
 	if (readl(priv->base + SDMMC_CMD) & SDMMC_CMD_CPSMEN)
 		writel(0, priv->base + SDMMC_CMD);
 
@@ -251,6 +252,26 @@ static void stm32_sdmmc2_start_cmd(struct stm32_sdmmc2_priv *priv,
 			cmd_param |= SDMMC_CMD_WAITRESP_1;
 	}
 
+	/*
+	 * SDMMC_DTIME must be set in two case:
+	 * - on data transfert.
+	 * - on busy request.
+	 * If not done or too short, the dtimeout flag occurs and DPSM stays
+	 * enabled/busy and waits for abort (stop transmission cmd).
+	 * Next data command is not possible whereas DPSM is activated.
+	 */
+	if (ctx->data_length) {
+		timeout = SDMMC_CMD_TIMEOUT;
+	} else {
+		writel(0, priv->base + SDMMC_DCTRL);
+
+		if (cmd->resp_type & MMC_RSP_BUSY)
+			timeout = SDMMC_CMD_TIMEOUT;
+	}
+
+	/* Set the SDMMC Data TimeOut value */
+	writel(timeout, priv->base + SDMMC_DTIMER);
+
 	/* Clear flags */
 	writel(SDMMC_ICR_STATIC_FLAGS, priv->base + SDMMC_ICR);
 
@@ -309,6 +330,31 @@ static int stm32_sdmmc2_end_cmd(struct stm32_sdmmc2_priv *priv,
 			cmd->response[2] = readl(priv->base + SDMMC_RESP3);
 			cmd->response[3] = readl(priv->base + SDMMC_RESP4);
 		}
+
+		/* Wait for BUSYD0END flag if busy status is detected */
+		if (cmd->resp_type & MMC_RSP_BUSY &&
+		    status & SDMMC_STA_BUSYD0) {
+			mask = SDMMC_STA_DTIMEOUT | SDMMC_STA_BUSYD0END;
+
+			/* Polling status register */
+			ret = readl_poll_timeout(priv->base + SDMMC_STA,
+						 status, status & mask,
+						 SDMMC_BUSYD0END_TIMEOUT_US);
+
+			if (ret < 0) {
+				debug("%s: timeout reading SDMMC_STA\n",
+				      __func__);
+				ctx->dpsm_abort = true;
+				return ret;
+			}
+
+			if (status & SDMMC_STA_DTIMEOUT) {
+				debug("%s: error SDMMC_STA_DTIMEOUT (0x%x)\n",
+				      __func__, status);
+				ctx->dpsm_abort = true;
+				return -ETIMEDOUT;
+			}
+		}
 	}
 
 	return 0;
@@ -395,7 +441,7 @@ retry_cmd:
 		stm32_sdmmc2_start_data(priv, data, &ctx);
 	}
 
-	stm32_sdmmc2_start_cmd(priv, cmd, cmdat);
+	stm32_sdmmc2_start_cmd(priv, cmd, cmdat, &ctx);
 
 	debug("%s: send cmd %d data: 0x%x @ 0x%x\n",
 	      __func__, cmd->cmdidx,
@@ -425,7 +471,10 @@ retry_cmd:
 		debug("%s: send STOP command to abort dpsm treatments\n",
 		      __func__);
 
-		stm32_sdmmc2_start_cmd(priv, &stop_cmd, SDMMC_CMD_CMDSTOP);
+		ctx.data_length = 0;
+
+		stm32_sdmmc2_start_cmd(priv, &stop_cmd,
+				       SDMMC_CMD_CMDSTOP, &ctx);
 		stm32_sdmmc2_end_cmd(priv, &stop_cmd, &ctx);
 
 		writel(SDMMC_ICR_STATIC_FLAGS, priv->base + SDMMC_ICR);
@@ -585,11 +634,11 @@ static int stm32_sdmmc2_probe(struct udevice *dev)
 	if (priv->base == FDT_ADDR_T_NONE)
 		return -EINVAL;
 
-	if (dev_read_bool(dev, "st,negedge"))
+	if (dev_read_bool(dev, "st,neg-edge"))
 		priv->clk_reg_msk |= SDMMC_CLKCR_NEGEDGE;
-	if (dev_read_bool(dev, "st,dirpol"))
+	if (dev_read_bool(dev, "st,sig-dir"))
 		priv->pwr_reg_msk |= SDMMC_POWER_DIRPOL;
-	if (dev_read_bool(dev, "st,pin-ckin"))
+	if (dev_read_bool(dev, "st,use-ckin"))
 		priv->clk_reg_msk |= SDMMC_CLKCR_SELCLKRX_CKIN;
 
 	ret = clk_get_by_index(dev, 0, &priv->clk);
diff --git a/drivers/mtd/nand/raw/Kconfig b/drivers/mtd/nand/raw/Kconfig
index 008f7b4..0419574 100644
--- a/drivers/mtd/nand/raw/Kconfig
+++ b/drivers/mtd/nand/raw/Kconfig
@@ -243,6 +243,17 @@ config SYS_NAND_BUSWIDTH_16BIT
 	    not available while configuring controller. So a static CONFIG_NAND_xx
 	    is needed to know the device's bus-width in advance.
 
+config NAND_STM32_FMC2
+	bool "STM32 FMC2 NAND driver"
+	depends on ARCH_STM32MP && OF_CONTROL && MTD
+	select SYS_NAND_SELF_INIT
+	imply CMD_NAND
+	help
+	  Enables support for NAND Flash chips on SoCs containing the FMC2
+	  NAND controller. This controller is found on STM32MP SoCs.
+	  The controller supports a maximum 8k page size and supports
+	  a maximum 8-bit correction error per sector of 512 bytes.
+
 if SPL
 
 config SYS_NAND_U_BOOT_LOCATIONS
diff --git a/drivers/mtd/nand/raw/Makefile b/drivers/mtd/nand/raw/Makefile
index c61e3f3..b10e718 100644
--- a/drivers/mtd/nand/raw/Makefile
+++ b/drivers/mtd/nand/raw/Makefile
@@ -65,6 +65,7 @@ obj-$(CONFIG_NAND_OMAP_ELM) += omap_elm.o
 obj-$(CONFIG_NAND_PLAT) += nand_plat.o
 obj-$(CONFIG_NAND_SUNXI) += sunxi_nand.o
 obj-$(CONFIG_NAND_ZYNQ) += zynq_nand.o
+obj-$(CONFIG_NAND_STM32_FMC2) += stm32_fmc2_nand.o
 
 else  # minimal SPL drivers
 
diff --git a/drivers/mtd/nand/raw/nand_ids.c b/drivers/mtd/nand/raw/nand_ids.c
index 4009d64..d457c54 100644
--- a/drivers/mtd/nand/raw/nand_ids.c
+++ b/drivers/mtd/nand/raw/nand_ids.c
@@ -52,6 +52,10 @@ struct nand_flash_dev nand_flash_ids[] = {
 	{"TC58NVG3S0F 8G 3.3V 8-bit",
 		{ .id = {0x98, 0xd3, 0x90, 0x26, 0x76, 0x15, 0x02, 0x08} },
 		  SZ_4K, SZ_1K, SZ_256K, 0, 8, 232, NAND_ECC_INFO(4, SZ_512) },
+	{"S34ML16G2 8G 3.3V 8-bit",
+		{ .id = {0x01, 0xd3, 0xd1, 0x95, 0x5a} },
+		  SZ_2K, SZ_1K, SZ_128K, 0, 5, 128, NAND_ECC_INFO(4, SZ_512),
+		  4 },
 	{"TC58NVG5D2 32G 3.3V 8-bit",
 		{ .id = {0x98, 0xd7, 0x94, 0x32, 0x76, 0x56, 0x09, 0x00} },
 		  SZ_8K, SZ_4K, SZ_1M, 0, 8, 640, NAND_ECC_INFO(40, SZ_1K) },
diff --git a/drivers/mtd/nand/raw/stm32_fmc2_nand.c b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
new file mode 100644
index 0000000..6f6f5e6
--- /dev/null
+++ b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
@@ -0,0 +1,1092 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ * Author: Christophe Kerello <christophe.kerello@st.com>
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <nand.h>
+#include <reset.h>
+#include <syscon.h>
+#include <asm/io.h>
+#include <dm/pinctrl.h>
+#include <linux/bitops.h>
+#include <linux/iopoll.h>
+#include <linux/ioport.h>
+#include <asm/arch/stm32.h>
+
+/* Bad block marker length */
+#define FMC2_BBM_LEN			2
+
+/* ECC step size */
+#define FMC2_ECC_STEP_SIZE		512
+
+/* Command delay */
+#define FMC2_RB_DELAY_US		30
+
+/* Max chip enable */
+#define FMC2_MAX_CE			2
+
+/* Timings */
+#define FMC2_THIZ			1
+#define FMC2_TIO			8000
+#define FMC2_TSYNC			3000
+#define FMC2_PCR_TIMING_MASK		0xf
+#define FMC2_PMEM_PATT_TIMING_MASK	0xff
+
+/* FMC2 Controller Registers */
+#define FMC2_BCR1			0x0
+#define FMC2_PCR			0x80
+#define FMC2_SR				0x84
+#define FMC2_PMEM			0x88
+#define FMC2_PATT			0x8c
+#define FMC2_HECCR			0x94
+#define FMC2_BCHISR			0x254
+#define FMC2_BCHICR			0x258
+#define FMC2_BCHPBR1			0x260
+#define FMC2_BCHPBR2			0x264
+#define FMC2_BCHPBR3			0x268
+#define FMC2_BCHPBR4			0x26c
+#define FMC2_BCHDSR0			0x27c
+#define FMC2_BCHDSR1			0x280
+#define FMC2_BCHDSR2			0x284
+#define FMC2_BCHDSR3			0x288
+#define FMC2_BCHDSR4			0x28c
+
+/* Register: FMC2_BCR1 */
+#define FMC2_BCR1_FMC2EN		BIT(31)
+
+/* Register: FMC2_PCR */
+#define FMC2_PCR_PWAITEN		BIT(1)
+#define FMC2_PCR_PBKEN			BIT(2)
+#define FMC2_PCR_PWID_MASK		GENMASK(5, 4)
+#define FMC2_PCR_PWID(x)		(((x) & 0x3) << 4)
+#define FMC2_PCR_PWID_BUSWIDTH_8	0
+#define FMC2_PCR_PWID_BUSWIDTH_16	1
+#define FMC2_PCR_ECCEN			BIT(6)
+#define FMC2_PCR_ECCALG			BIT(8)
+#define FMC2_PCR_TCLR_MASK		GENMASK(12, 9)
+#define FMC2_PCR_TCLR(x)		(((x) & 0xf) << 9)
+#define FMC2_PCR_TCLR_DEFAULT		0xf
+#define FMC2_PCR_TAR_MASK		GENMASK(16, 13)
+#define FMC2_PCR_TAR(x)			(((x) & 0xf) << 13)
+#define FMC2_PCR_TAR_DEFAULT		0xf
+#define FMC2_PCR_ECCSS_MASK		GENMASK(19, 17)
+#define FMC2_PCR_ECCSS(x)		(((x) & 0x7) << 17)
+#define FMC2_PCR_ECCSS_512		1
+#define FMC2_PCR_ECCSS_2048		3
+#define FMC2_PCR_BCHECC			BIT(24)
+#define FMC2_PCR_WEN			BIT(25)
+
+/* Register: FMC2_SR */
+#define FMC2_SR_NWRF			BIT(6)
+
+/* Register: FMC2_PMEM */
+#define FMC2_PMEM_MEMSET(x)		(((x) & 0xff) << 0)
+#define FMC2_PMEM_MEMWAIT(x)		(((x) & 0xff) << 8)
+#define FMC2_PMEM_MEMHOLD(x)		(((x) & 0xff) << 16)
+#define FMC2_PMEM_MEMHIZ(x)		(((x) & 0xff) << 24)
+#define FMC2_PMEM_DEFAULT		0x0a0a0a0a
+
+/* Register: FMC2_PATT */
+#define FMC2_PATT_ATTSET(x)		(((x) & 0xff) << 0)
+#define FMC2_PATT_ATTWAIT(x)		(((x) & 0xff) << 8)
+#define FMC2_PATT_ATTHOLD(x)		(((x) & 0xff) << 16)
+#define FMC2_PATT_ATTHIZ(x)		(((x) & 0xff) << 24)
+#define FMC2_PATT_DEFAULT		0x0a0a0a0a
+
+/* Register: FMC2_BCHISR */
+#define FMC2_BCHISR_DERF		BIT(1)
+#define FMC2_BCHISR_EPBRF		BIT(4)
+
+/* Register: FMC2_BCHICR */
+#define FMC2_BCHICR_CLEAR_IRQ		GENMASK(4, 0)
+
+/* Register: FMC2_BCHDSR0 */
+#define FMC2_BCHDSR0_DUE		BIT(0)
+#define FMC2_BCHDSR0_DEF		BIT(1)
+#define FMC2_BCHDSR0_DEN_MASK		0xf0
+#define FMC2_BCHDSR0_DEN_SHIFT		4
+
+/* Register: FMC2_BCHDSR1 */
+#define FMC2_BCHDSR1_EBP1_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR1_EBP2_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR1_EBP2_SHIFT		16
+
+/* Register: FMC2_BCHDSR2 */
+#define FMC2_BCHDSR2_EBP3_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR2_EBP4_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR2_EBP4_SHIFT		16
+
+/* Register: FMC2_BCHDSR3 */
+#define FMC2_BCHDSR3_EBP5_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR3_EBP6_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR3_EBP6_SHIFT		16
+
+/* Register: FMC2_BCHDSR4 */
+#define FMC2_BCHDSR4_EBP7_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR4_EBP8_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR4_EBP8_SHIFT		16
+
+#define FMC2_NSEC_PER_SEC		1000000000L
+
+enum stm32_fmc2_ecc {
+	FMC2_ECC_HAM = 1,
+	FMC2_ECC_BCH4 = 4,
+	FMC2_ECC_BCH8 = 8
+};
+
+struct stm32_fmc2_timings {
+	u8 tclr;
+	u8 tar;
+	u8 thiz;
+	u8 twait;
+	u8 thold_mem;
+	u8 tset_mem;
+	u8 thold_att;
+	u8 tset_att;
+};
+
+struct stm32_fmc2_nand {
+	struct nand_chip chip;
+	struct stm32_fmc2_timings timings;
+	int ncs;
+	int cs_used[FMC2_MAX_CE];
+};
+
+static inline struct stm32_fmc2_nand *to_fmc2_nand(struct nand_chip *chip)
+{
+	return container_of(chip, struct stm32_fmc2_nand, chip);
+}
+
+struct stm32_fmc2_nfc {
+	struct nand_hw_control base;
+	struct stm32_fmc2_nand nand;
+	struct nand_ecclayout ecclayout;
+	void __iomem *io_base;
+	void __iomem *data_base[FMC2_MAX_CE];
+	void __iomem *cmd_base[FMC2_MAX_CE];
+	void __iomem *addr_base[FMC2_MAX_CE];
+	struct clk clk;
+
+	u8 cs_assigned;
+	int cs_sel;
+};
+
+static inline struct stm32_fmc2_nfc *to_stm32_nfc(struct nand_hw_control *base)
+{
+	return container_of(base, struct stm32_fmc2_nfc, base);
+}
+
+/* Clear interrupt sources in case of bch is used */
+static inline void stm32_fmc2_clear_bch_irq(struct stm32_fmc2_nfc *fmc2)
+{
+	writel(FMC2_BCHICR_CLEAR_IRQ, fmc2->io_base + FMC2_BCHICR);
+}
+
+/* Set bus width to 16-bit or 8-bit */
+static void stm32_fmc2_set_buswidth_16(struct stm32_fmc2_nfc *fmc2, bool set)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	pcr &= ~FMC2_PCR_PWID_MASK;
+	if (set)
+		pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_BUSWIDTH_16);
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Enable/disable ecc */
+static void stm32_fmc2_set_ecc(struct stm32_fmc2_nfc *fmc2, bool enable)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	pcr &= ~FMC2_PCR_ECCEN;
+	if (enable)
+		pcr |= FMC2_PCR_ECCEN;
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Send command and address cycles */
+static void stm32_fmc2_cmd_ctrl(struct mtd_info *mtd, int cmd,
+				unsigned int ctrl)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+
+	if (cmd == NAND_CMD_NONE)
+		return;
+
+	if (ctrl & NAND_CLE) {
+		writeb(cmd, fmc2->cmd_base[fmc2->cs_sel]);
+		return;
+	}
+
+	writeb(cmd, fmc2->addr_base[fmc2->cs_sel]);
+}
+
+/*
+ * Enable ecc logic and reset syndrome/parity bits previously calculated
+ * Syndrome/parity bits is cleared by setting the ECCEN bit to 0
+ */
+static void stm32_fmc2_hwctl(struct mtd_info *mtd, int mode)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	if (chip->ecc.strength != FMC2_ECC_HAM) {
+		u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+		if (mode == NAND_ECC_WRITE)
+			pcr |= FMC2_PCR_WEN;
+		else
+			pcr &= ~FMC2_PCR_WEN;
+		writel(pcr, fmc2->io_base + FMC2_PCR);
+
+		stm32_fmc2_clear_bch_irq(fmc2);
+	}
+
+	stm32_fmc2_set_ecc(fmc2, true);
+}
+
+/*
+ * Ecc Hamming calculation
+ * Ecc is 3 bytes for 512 bytes of data (supports error correction up to
+ * max of 1-bit)
+ */
+static int stm32_fmc2_ham_calculate(struct mtd_info *mtd, const u8 *data,
+				    u8 *ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 heccr, sr;
+	int ret;
+
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_SR, sr,
+				 sr & FMC2_SR_NWRF, 10000);
+	if (ret < 0) {
+		pr_err("Ham timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	heccr = readl(fmc2->io_base + FMC2_HECCR);
+
+	ecc[0] = heccr;
+	ecc[1] = heccr >> 8;
+	ecc[2] = heccr >> 16;
+
+	/* Disable ecc */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	return 0;
+}
+
+static int stm32_fmc2_ham_correct(struct mtd_info *mtd, u8 *dat,
+				  u8 *read_ecc, u8 *calc_ecc)
+{
+	u8 bit_position = 0, b0, b1, b2;
+	u32 byte_addr = 0, b;
+	u32 i, shifting = 1;
+
+	/* Indicate which bit and byte is faulty (if any) */
+	b0 = read_ecc[0] ^ calc_ecc[0];
+	b1 = read_ecc[1] ^ calc_ecc[1];
+	b2 = read_ecc[2] ^ calc_ecc[2];
+	b = b0 | (b1 << 8) | (b2 << 16);
+
+	/* No errors */
+	if (likely(!b))
+		return 0;
+
+	/* Calculate bit position */
+	for (i = 0; i < 3; i++) {
+		switch (b % 4) {
+		case 2:
+			bit_position += shifting;
+		case 1:
+			break;
+		default:
+			return -EBADMSG;
+		}
+		shifting <<= 1;
+		b >>= 2;
+	}
+
+	/* Calculate byte position */
+	shifting = 1;
+	for (i = 0; i < 9; i++) {
+		switch (b % 4) {
+		case 2:
+			byte_addr += shifting;
+		case 1:
+			break;
+		default:
+			return -EBADMSG;
+		}
+		shifting <<= 1;
+		b >>= 2;
+	}
+
+	/* Flip the bit */
+	dat[byte_addr] ^= (1 << bit_position);
+
+	return 1;
+}
+
+/*
+ * Ecc BCH calculation and correction
+ * Ecc is 7/13 bytes for 512 bytes of data (supports error correction up to
+ * max of 4-bit/8-bit)
+ */
+
+static int stm32_fmc2_bch_calculate(struct mtd_info *mtd, const u8 *data,
+				    u8 *ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 bchpbr, bchisr;
+	int ret;
+
+	/* Wait that the BCH encoder parity is available */
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_BCHISR, bchisr,
+				 bchisr & FMC2_BCHISR_EPBRF, 10000);
+	if (ret < 0) {
+		pr_err("Bch timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	/* Read parity bits (write) or syndrome (read) */
+	bchpbr = readl(fmc2->io_base + FMC2_BCHPBR1);
+	ecc[0] = bchpbr;
+	ecc[1] = bchpbr >> 8;
+	ecc[2] = bchpbr >> 16;
+	ecc[3] = bchpbr >> 24;
+
+	bchpbr = readl(fmc2->io_base + FMC2_BCHPBR2);
+	ecc[4] = bchpbr;
+	ecc[5] = bchpbr >> 8;
+	ecc[6] = bchpbr >> 16;
+
+	if (chip->ecc.strength == FMC2_ECC_BCH8) {
+		ecc[7] = bchpbr >> 24;
+
+		bchpbr = readl(fmc2->io_base + FMC2_BCHPBR3);
+		ecc[8] = bchpbr;
+		ecc[9] = bchpbr >> 8;
+		ecc[10] = bchpbr >> 16;
+		ecc[11] = bchpbr >> 24;
+
+		bchpbr = readl(fmc2->io_base + FMC2_BCHPBR4);
+		ecc[12] = bchpbr;
+	}
+
+	/* Disable ecc */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	return 0;
+}
+
+/* BCH algorithm correction */
+static int stm32_fmc2_bch_correct(struct mtd_info *mtd, u8 *dat,
+				  u8 *read_ecc, u8 *calc_ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 bchdsr0, bchdsr1, bchdsr2, bchdsr3, bchdsr4, bchisr;
+	u16 pos[8];
+	int i, ret, den, eccsize = chip->ecc.size;
+	unsigned int nb_errs = 0;
+
+	/* Wait that the BCH encoder syndrome is available */
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_BCHISR, bchisr,
+				 bchisr & FMC2_BCHISR_DERF, 10000);
+	if (ret < 0) {
+		pr_err("Bch timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	bchdsr0 = readl(fmc2->io_base + FMC2_BCHDSR0);
+	bchdsr1 = readl(fmc2->io_base + FMC2_BCHDSR1);
+	bchdsr2 = readl(fmc2->io_base + FMC2_BCHDSR2);
+	bchdsr3 = readl(fmc2->io_base + FMC2_BCHDSR3);
+	bchdsr4 = readl(fmc2->io_base + FMC2_BCHDSR4);
+
+	/* Disable ecc */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	/* No errors found */
+	if (likely(!(bchdsr0 & FMC2_BCHDSR0_DEF)))
+		return 0;
+
+	/* Too many errors detected */
+	if (unlikely(bchdsr0 & FMC2_BCHDSR0_DUE))
+		return -EBADMSG;
+
+	pos[0] = bchdsr1 & FMC2_BCHDSR1_EBP1_MASK;
+	pos[1] = (bchdsr1 & FMC2_BCHDSR1_EBP2_MASK) >> FMC2_BCHDSR1_EBP2_SHIFT;
+	pos[2] = bchdsr2 & FMC2_BCHDSR2_EBP3_MASK;
+	pos[3] = (bchdsr2 & FMC2_BCHDSR2_EBP4_MASK) >> FMC2_BCHDSR2_EBP4_SHIFT;
+	pos[4] = bchdsr3 & FMC2_BCHDSR3_EBP5_MASK;
+	pos[5] = (bchdsr3 & FMC2_BCHDSR3_EBP6_MASK) >> FMC2_BCHDSR3_EBP6_SHIFT;
+	pos[6] = bchdsr4 & FMC2_BCHDSR4_EBP7_MASK;
+	pos[7] = (bchdsr4 & FMC2_BCHDSR4_EBP8_MASK) >> FMC2_BCHDSR4_EBP8_SHIFT;
+
+	den = (bchdsr0 & FMC2_BCHDSR0_DEN_MASK) >> FMC2_BCHDSR0_DEN_SHIFT;
+	for (i = 0; i < den; i++) {
+		if (pos[i] < eccsize * 8) {
+			__change_bit(pos[i], (unsigned long *)dat);
+			nb_errs++;
+		}
+	}
+
+	return nb_errs;
+}
+
+static int stm32_fmc2_read_page(struct mtd_info *mtd,
+				struct nand_chip *chip, u8 *buf,
+				int oob_required, int page)
+{
+	int i, s, stat, eccsize = chip->ecc.size;
+	int eccbytes = chip->ecc.bytes;
+	int eccsteps = chip->ecc.steps;
+	int eccstrength = chip->ecc.strength;
+	u8 *p = buf;
+	u8 *ecc_calc = chip->buffers->ecccalc;
+	u8 *ecc_code = chip->buffers->ecccode;
+	unsigned int max_bitflips = 0;
+
+	for (i = mtd->writesize + FMC2_BBM_LEN, s = 0; s < eccsteps;
+	     s++, i += eccbytes, p += eccsize) {
+		chip->ecc.hwctl(mtd, NAND_ECC_READ);
+
+		/* Read the nand page sector (512 bytes) */
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, s * eccsize, -1);
+		chip->read_buf(mtd, p, eccsize);
+
+		/* Read the corresponding ecc bytes */
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, i, -1);
+		chip->read_buf(mtd, ecc_code, eccbytes);
+
+		/* Correct the data */
+		stat = chip->ecc.correct(mtd, p, ecc_code, ecc_calc);
+		if (stat == -EBADMSG)
+			/* Check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+							   ecc_code, eccbytes,
+							   NULL, 0,
+							   eccstrength);
+
+		if (stat < 0) {
+			mtd->ecc_stats.failed++;
+		} else {
+			mtd->ecc_stats.corrected += stat;
+			max_bitflips = max_t(unsigned int, max_bitflips, stat);
+		}
+	}
+
+	/* Read oob */
+	chip->cmdfunc(mtd, NAND_CMD_RNDOUT, mtd->writesize, -1);
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return max_bitflips;
+}
+
+/* Timings configuration */
+static void stm32_fmc2_timings_init(struct nand_chip *chip)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+	struct stm32_fmc2_timings *timings = &nand->timings;
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+	u32 pmem, patt;
+
+	/* Set tclr/tar timings */
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(timings->tclr);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(timings->tar);
+
+	/* Set tset/twait/thold/thiz timings in common bank */
+	pmem = FMC2_PMEM_MEMSET(timings->tset_mem);
+	pmem |= FMC2_PMEM_MEMWAIT(timings->twait);
+	pmem |= FMC2_PMEM_MEMHOLD(timings->thold_mem);
+	pmem |= FMC2_PMEM_MEMHIZ(timings->thiz);
+
+	/* Set tset/twait/thold/thiz timings in attribut bank */
+	patt = FMC2_PATT_ATTSET(timings->tset_att);
+	patt |= FMC2_PATT_ATTWAIT(timings->twait);
+	patt |= FMC2_PATT_ATTHOLD(timings->thold_att);
+	patt |= FMC2_PATT_ATTHIZ(timings->thiz);
+
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+	writel(pmem, fmc2->io_base + FMC2_PMEM);
+	writel(patt, fmc2->io_base + FMC2_PATT);
+}
+
+/* Controller initialization */
+static void stm32_fmc2_init(struct stm32_fmc2_nfc *fmc2)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+	u32 bcr1 = readl(fmc2->io_base + FMC2_BCR1);
+
+	/* Set CS used to undefined */
+	fmc2->cs_sel = -1;
+
+	/* Enable wait feature and nand flash memory bank */
+	pcr |= FMC2_PCR_PWAITEN;
+	pcr |= FMC2_PCR_PBKEN;
+
+	/* Set buswidth to 8 bits mode for identification */
+	pcr &= ~FMC2_PCR_PWID_MASK;
+
+	/* Ecc logic is disabled */
+	pcr &= ~FMC2_PCR_ECCEN;
+
+	/* Default mode */
+	pcr &= ~FMC2_PCR_ECCALG;
+	pcr &= ~FMC2_PCR_BCHECC;
+	pcr &= ~FMC2_PCR_WEN;
+
+	/* Set default ecc sector size */
+	pcr &= ~FMC2_PCR_ECCSS_MASK;
+	pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_2048);
+
+	/* Set default tclr/tar timings */
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(FMC2_PCR_TCLR_DEFAULT);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(FMC2_PCR_TAR_DEFAULT);
+
+	/* Enable FMC2 controller */
+	bcr1 |= FMC2_BCR1_FMC2EN;
+
+	writel(bcr1, fmc2->io_base + FMC2_BCR1);
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+	writel(FMC2_PMEM_DEFAULT, fmc2->io_base + FMC2_PMEM);
+	writel(FMC2_PATT_DEFAULT, fmc2->io_base + FMC2_PATT);
+}
+
+/* Controller configuration */
+static void stm32_fmc2_setup(struct nand_chip *chip)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	/* Configure in Hamming by default */
+	if (chip->ecc.strength == FMC2_ECC_BCH8) {
+		pcr |= FMC2_PCR_ECCALG;
+		pcr |= FMC2_PCR_BCHECC;
+	} else if (chip->ecc.strength == FMC2_ECC_BCH4) {
+		pcr |= FMC2_PCR_ECCALG;
+	}
+
+	/* Set buswidth */
+	if (chip->options & NAND_BUSWIDTH_16)
+		pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_BUSWIDTH_16);
+
+	/* Set ecc sector size */
+	pcr &= ~FMC2_PCR_ECCSS_MASK;
+	pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_512);
+
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Select function */
+static void stm32_fmc2_select_chip(struct mtd_info *mtd, int chipnr)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+
+	if (chipnr < 0 || chipnr >= nand->ncs)
+		return;
+
+	if (nand->cs_used[chipnr] == fmc2->cs_sel)
+		return;
+
+	fmc2->cs_sel = nand->cs_used[chipnr];
+	chip->IO_ADDR_R = fmc2->data_base[fmc2->cs_sel];
+	chip->IO_ADDR_W = fmc2->data_base[fmc2->cs_sel];
+
+	/* FMC2 setup routine */
+	stm32_fmc2_setup(chip);
+
+	/* Apply timings */
+	stm32_fmc2_timings_init(chip);
+}
+
+/* Controller timings */
+static void stm32_fmc2_calc_timings(struct nand_chip *chip,
+				    const struct nand_sdr_timings *sdrt)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+	struct stm32_fmc2_timings *tims = &nand->timings;
+	unsigned long hclk = clk_get_rate(&fmc2->clk);
+	unsigned long hclkp = FMC2_NSEC_PER_SEC / (hclk / 1000);
+	int tar, tclr, thiz, twait, tset_mem, tset_att, thold_mem, thold_att;
+
+	tar = hclkp;
+	if (tar < sdrt->tAR_min)
+		tar = sdrt->tAR_min;
+	tims->tar = DIV_ROUND_UP(tar, hclkp) - 1;
+	if (tims->tar > FMC2_PCR_TIMING_MASK)
+		tims->tar = FMC2_PCR_TIMING_MASK;
+
+	tclr = hclkp;
+	if (tclr < sdrt->tCLR_min)
+		tclr = sdrt->tCLR_min;
+	tims->tclr = DIV_ROUND_UP(tclr, hclkp) - 1;
+	if (tims->tclr > FMC2_PCR_TIMING_MASK)
+		tims->tclr = FMC2_PCR_TIMING_MASK;
+
+	tims->thiz = FMC2_THIZ;
+	thiz = (tims->thiz + 1) * hclkp;
+
+	/*
+	 * tWAIT > tRP
+	 * tWAIT > tWP
+	 * tWAIT > tREA + tIO
+	 */
+	twait = hclkp;
+	if (twait < sdrt->tRP_min)
+		twait = sdrt->tRP_min;
+	if (twait < sdrt->tWP_min)
+		twait = sdrt->tWP_min;
+	if (twait < sdrt->tREA_max + FMC2_TIO)
+		twait = sdrt->tREA_max + FMC2_TIO;
+	tims->twait = DIV_ROUND_UP(twait, hclkp);
+	if (tims->twait == 0)
+		tims->twait = 1;
+	else if (tims->twait > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->twait = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tSETUP_MEM > tCS - tWAIT
+	 * tSETUP_MEM > tALS - tWAIT
+	 * tSETUP_MEM > tDS - (tWAIT - tHIZ)
+	 */
+	tset_mem = hclkp;
+	if (sdrt->tCS_min > twait && (tset_mem < sdrt->tCS_min - twait))
+		tset_mem = sdrt->tCS_min - twait;
+	if (sdrt->tALS_min > twait && (tset_mem < sdrt->tALS_min - twait))
+		tset_mem = sdrt->tALS_min - twait;
+	if (twait > thiz && (sdrt->tDS_min > twait - thiz) &&
+	    (tset_mem < sdrt->tDS_min - (twait - thiz)))
+		tset_mem = sdrt->tDS_min - (twait - thiz);
+	tims->tset_mem = DIV_ROUND_UP(tset_mem, hclkp);
+	if (tims->tset_mem == 0)
+		tims->tset_mem = 1;
+	else if (tims->tset_mem > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->tset_mem = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tHOLD_MEM > tCH
+	 * tHOLD_MEM > tREH - tSETUP_MEM
+	 * tHOLD_MEM > max(tRC, tWC) - (tSETUP_MEM + tWAIT)
+	 */
+	thold_mem = hclkp;
+	if (thold_mem < sdrt->tCH_min)
+		thold_mem = sdrt->tCH_min;
+	if (sdrt->tREH_min > tset_mem &&
+	    (thold_mem < sdrt->tREH_min - tset_mem))
+		thold_mem = sdrt->tREH_min - tset_mem;
+	if ((sdrt->tRC_min > tset_mem + twait) &&
+	    (thold_mem < sdrt->tRC_min - (tset_mem + twait)))
+		thold_mem = sdrt->tRC_min - (tset_mem + twait);
+	if ((sdrt->tWC_min > tset_mem + twait) &&
+	    (thold_mem < sdrt->tWC_min - (tset_mem + twait)))
+		thold_mem = sdrt->tWC_min - (tset_mem + twait);
+	tims->thold_mem = DIV_ROUND_UP(thold_mem, hclkp);
+	if (tims->thold_mem == 0)
+		tims->thold_mem = 1;
+	else if (tims->thold_mem > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->thold_mem = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tSETUP_ATT > tCS - tWAIT
+	 * tSETUP_ATT > tCLS - tWAIT
+	 * tSETUP_ATT > tALS - tWAIT
+	 * tSETUP_ATT > tRHW - tHOLD_MEM
+	 * tSETUP_ATT > tDS - (tWAIT - tHIZ)
+	 */
+	tset_att = hclkp;
+	if (sdrt->tCS_min > twait && (tset_att < sdrt->tCS_min - twait))
+		tset_att = sdrt->tCS_min - twait;
+	if (sdrt->tCLS_min > twait && (tset_att < sdrt->tCLS_min - twait))
+		tset_att = sdrt->tCLS_min - twait;
+	if (sdrt->tALS_min > twait && (tset_att < sdrt->tALS_min - twait))
+		tset_att = sdrt->tALS_min - twait;
+	if (sdrt->tRHW_min > thold_mem &&
+	    (tset_att < sdrt->tRHW_min - thold_mem))
+		tset_att = sdrt->tRHW_min - thold_mem;
+	if (twait > thiz && (sdrt->tDS_min > twait - thiz) &&
+	    (tset_att < sdrt->tDS_min - (twait - thiz)))
+		tset_att = sdrt->tDS_min - (twait - thiz);
+	tims->tset_att = DIV_ROUND_UP(tset_att, hclkp);
+	if (tims->tset_att == 0)
+		tims->tset_att = 1;
+	else if (tims->tset_att > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->tset_att = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tHOLD_ATT > tALH
+	 * tHOLD_ATT > tCH
+	 * tHOLD_ATT > tCLH
+	 * tHOLD_ATT > tCOH
+	 * tHOLD_ATT > tDH
+	 * tHOLD_ATT > tWB + tIO + tSYNC - tSETUP_MEM
+	 * tHOLD_ATT > tADL - tSETUP_MEM
+	 * tHOLD_ATT > tWH - tSETUP_MEM
+	 * tHOLD_ATT > tWHR - tSETUP_MEM
+	 * tHOLD_ATT > tRC - (tSETUP_ATT + tWAIT)
+	 * tHOLD_ATT > tWC - (tSETUP_ATT + tWAIT)
+	 */
+	thold_att = hclkp;
+	if (thold_att < sdrt->tALH_min)
+		thold_att = sdrt->tALH_min;
+	if (thold_att < sdrt->tCH_min)
+		thold_att = sdrt->tCH_min;
+	if (thold_att < sdrt->tCLH_min)
+		thold_att = sdrt->tCLH_min;
+	if (thold_att < sdrt->tCOH_min)
+		thold_att = sdrt->tCOH_min;
+	if (thold_att < sdrt->tDH_min)
+		thold_att = sdrt->tDH_min;
+	if ((sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC > tset_mem) &&
+	    (thold_att < sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC - tset_mem))
+		thold_att = sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC - tset_mem;
+	if (sdrt->tADL_min > tset_mem &&
+	    (thold_att < sdrt->tADL_min - tset_mem))
+		thold_att = sdrt->tADL_min - tset_mem;
+	if (sdrt->tWH_min > tset_mem &&
+	    (thold_att < sdrt->tWH_min - tset_mem))
+		thold_att = sdrt->tWH_min - tset_mem;
+	if (sdrt->tWHR_min > tset_mem &&
+	    (thold_att < sdrt->tWHR_min - tset_mem))
+		thold_att = sdrt->tWHR_min - tset_mem;
+	if ((sdrt->tRC_min > tset_att + twait) &&
+	    (thold_att < sdrt->tRC_min - (tset_att + twait)))
+		thold_att = sdrt->tRC_min - (tset_att + twait);
+	if ((sdrt->tWC_min > tset_att + twait) &&
+	    (thold_att < sdrt->tWC_min - (tset_att + twait)))
+		thold_att = sdrt->tWC_min - (tset_att + twait);
+	tims->thold_att = DIV_ROUND_UP(thold_att, hclkp);
+	if (tims->thold_att == 0)
+		tims->thold_att = 1;
+	else if (tims->thold_att > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->thold_att = FMC2_PMEM_PATT_TIMING_MASK;
+}
+
+static int stm32_fmc2_setup_interface(struct mtd_info *mtd, int chipnr,
+				      const struct nand_data_interface *conf)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	const struct nand_sdr_timings *sdrt;
+
+	sdrt = nand_get_sdr_timings(conf);
+	if (IS_ERR(sdrt))
+		return PTR_ERR(sdrt);
+
+	if (chipnr == NAND_DATA_IFACE_CHECK_ONLY)
+		return 0;
+
+	stm32_fmc2_calc_timings(chip, sdrt);
+
+	/* Apply timings */
+	stm32_fmc2_timings_init(chip);
+
+	return 0;
+}
+
+/* NAND callbacks setup */
+static void stm32_fmc2_nand_callbacks_setup(struct nand_chip *chip)
+{
+	chip->ecc.hwctl = stm32_fmc2_hwctl;
+
+	/*
+	 * Specific callbacks to read/write a page depending on
+	 * the algo used (Hamming, BCH).
+	 */
+	if (chip->ecc.strength == FMC2_ECC_HAM) {
+		/* Hamming is used */
+		chip->ecc.calculate = stm32_fmc2_ham_calculate;
+		chip->ecc.correct = stm32_fmc2_ham_correct;
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 4 : 3;
+		chip->ecc.options |= NAND_ECC_GENERIC_ERASED_CHECK;
+		return;
+	}
+
+	/* BCH is used */
+	chip->ecc.read_page = stm32_fmc2_read_page;
+	chip->ecc.calculate = stm32_fmc2_bch_calculate;
+	chip->ecc.correct = stm32_fmc2_bch_correct;
+
+	if (chip->ecc.strength == FMC2_ECC_BCH8)
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 14 : 13;
+	else
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 8 : 7;
+}
+
+/* FMC2 caps */
+static int stm32_fmc2_calc_ecc_bytes(int step_size, int strength)
+{
+	/* Hamming */
+	if (strength == FMC2_ECC_HAM)
+		return 4;
+
+	/* BCH8 */
+	if (strength == FMC2_ECC_BCH8)
+		return 14;
+
+	/* BCH4 */
+	return 8;
+}
+
+NAND_ECC_CAPS_SINGLE(stm32_fmc2_ecc_caps, stm32_fmc2_calc_ecc_bytes,
+		     FMC2_ECC_STEP_SIZE,
+		     FMC2_ECC_HAM, FMC2_ECC_BCH4, FMC2_ECC_BCH8);
+
+/* FMC2 probe */
+static int stm32_fmc2_parse_child(struct stm32_fmc2_nfc *fmc2,
+				  ofnode node)
+{
+	struct stm32_fmc2_nand *nand = &fmc2->nand;
+	u32 cs[FMC2_MAX_CE];
+	int ret, chip_cs;
+
+	if (!ofnode_get_property(node, "reg", &nand->ncs))
+		return -EINVAL;
+
+	nand->ncs /= sizeof(u32);
+	if (!nand->ncs) {
+		pr_err("Invalid reg property size\n");
+		return -EINVAL;
+	}
+
+	ret = ofnode_read_u32_array(node, "reg", cs, nand->ncs);
+	if (ret < 0) {
+		pr_err("Could not retrieve reg property\n");
+		return -EINVAL;
+	}
+
+	for (chip_cs = 0; chip_cs < nand->ncs; chip_cs++) {
+		if (cs[chip_cs] > FMC2_MAX_CE) {
+			pr_err("Invalid reg value: %d\n",
+			       nand->cs_used[chip_cs]);
+			return -EINVAL;
+		}
+
+		if (fmc2->cs_assigned & BIT(cs[chip_cs])) {
+			pr_err("Cs already assigned: %d\n",
+			       nand->cs_used[chip_cs]);
+			return -EINVAL;
+		}
+
+		fmc2->cs_assigned |= BIT(cs[chip_cs]);
+		nand->cs_used[chip_cs] = cs[chip_cs];
+	}
+
+	nand->chip.flash_node = ofnode_to_offset(node);
+
+	return 0;
+}
+
+static int stm32_fmc2_parse_children(struct udevice *dev,
+				     struct stm32_fmc2_nfc *fmc2)
+{
+	ofnode child;
+	int ret, nchips = 0;
+
+	dev_for_each_subnode(child, dev)
+		nchips++;
+
+	if (!nchips) {
+		pr_err("NAND chip not defined\n");
+		return -EINVAL;
+	}
+
+	if (nchips > 1) {
+		pr_err("Too many NAND chips defined\n");
+		return -EINVAL;
+	}
+
+	dev_for_each_subnode(child, dev) {
+		ret = stm32_fmc2_parse_child(fmc2, child);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_fmc2_probe(struct udevice *dev)
+{
+	struct stm32_fmc2_nfc *fmc2 = dev_get_priv(dev);
+	struct stm32_fmc2_nand *nand = &fmc2->nand;
+	struct nand_chip *chip = &nand->chip;
+	struct mtd_info *mtd = &chip->mtd;
+	struct nand_ecclayout *ecclayout;
+	struct resource resource;
+	struct reset_ctl reset;
+	int oob_index, chip_cs, mem_region, ret, i;
+
+	spin_lock_init(&fmc2->controller.lock);
+	init_waitqueue_head(&fmc2->controller.wq);
+
+	ret = stm32_fmc2_parse_children(dev, fmc2);
+	if (ret)
+		return ret;
+
+	/* Get resources */
+	ret = dev_read_resource(dev, 0, &resource);
+	if (ret) {
+		pr_err("Resource io_base not found");
+		return ret;
+	}
+	fmc2->io_base = (void __iomem *)resource.start;
+
+	for (chip_cs = 0, mem_region = 1; chip_cs < FMC2_MAX_CE;
+	     chip_cs++, mem_region += 3) {
+		if (!(fmc2->cs_assigned & BIT(chip_cs)))
+			continue;
+
+		ret = dev_read_resource(dev, mem_region, &resource);
+		if (ret) {
+			pr_err("Resource data_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->data_base[chip_cs] = (void __iomem *)resource.start;
+
+		ret = dev_read_resource(dev, mem_region + 1, &resource);
+		if (ret) {
+			pr_err("Resource cmd_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->cmd_base[chip_cs] = (void __iomem *)resource.start;
+
+		ret = dev_read_resource(dev, mem_region + 2, &resource);
+		if (ret) {
+			pr_err("Resource addr_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->addr_base[chip_cs] = (void __iomem *)resource.start;
+	}
+
+	/* Enable clock */
+	ret = clk_get_by_index(dev, 0, &fmc2->clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&fmc2->clk);
+	if (ret)
+		return ret;
+
+	/* Reset */
+	ret = reset_get_by_index(dev, 0, &reset);
+	if (!ret) {
+		reset_assert(&reset);
+		udelay(2);
+		reset_deassert(&reset);
+	}
+
+	/* FMC2 init routine */
+	stm32_fmc2_init(fmc2);
+
+	chip->controller = &fmc2->base;
+	chip->select_chip = stm32_fmc2_select_chip;
+	chip->setup_data_interface = stm32_fmc2_setup_interface;
+	chip->cmd_ctrl = stm32_fmc2_cmd_ctrl;
+	chip->chip_delay = FMC2_RB_DELAY_US;
+	chip->options |= NAND_BUSWIDTH_AUTO | NAND_NO_SUBPAGE_WRITE |
+			 NAND_USE_BOUNCE_BUFFER;
+
+	/* Default settings */
+	chip->ecc.mode = NAND_ECC_HW;
+	chip->ecc.size = FMC2_ECC_STEP_SIZE;
+	chip->ecc.strength = FMC2_ECC_BCH8;
+
+	/* Scan to find existence of the device */
+	ret = nand_scan_ident(mtd, nand->ncs, NULL);
+	if (ret)
+		return ret;
+
+	/*
+	 * Only NAND_ECC_HW mode is actually supported
+	 * Hamming => ecc.strength = 1
+	 * BCH4 => ecc.strength = 4
+	 * BCH8 => ecc.strength = 8
+	 * ecc sector size = 512
+	 */
+	if (chip->ecc.mode != NAND_ECC_HW) {
+		pr_err("Nand_ecc_mode is not well defined in the DT\n");
+		return -EINVAL;
+	}
+
+	ret = nand_check_ecc_caps(chip, &stm32_fmc2_ecc_caps,
+				  mtd->oobsize - FMC2_BBM_LEN);
+	if (ret) {
+		pr_err("No valid ecc settings set\n");
+		return ret;
+	}
+
+	if (chip->bbt_options & NAND_BBT_USE_FLASH)
+		chip->bbt_options |= NAND_BBT_NO_OOB;
+
+	/* NAND callbacks setup */
+	stm32_fmc2_nand_callbacks_setup(chip);
+
+	/* Define ecc layout */
+	ecclayout = &fmc2->ecclayout;
+	ecclayout->eccbytes = chip->ecc.bytes *
+			      (mtd->writesize / chip->ecc.size);
+	oob_index = FMC2_BBM_LEN;
+	for (i = 0; i < ecclayout->eccbytes; i++, oob_index++)
+		ecclayout->eccpos[i] = oob_index;
+	ecclayout->oobfree->offset = oob_index + 1;
+	ecclayout->oobfree->length = mtd->oobsize - ecclayout->oobfree->offset;
+	chip->ecc.layout = ecclayout;
+
+	/* Configure bus width to 16-bit */
+	if (chip->options & NAND_BUSWIDTH_16)
+		stm32_fmc2_set_buswidth_16(fmc2, true);
+
+	/* Scan the device to fill MTD data-structures */
+	ret = nand_scan_tail(mtd);
+	if (ret)
+		return ret;
+
+	return nand_register(0, mtd);
+}
+
+static const struct udevice_id stm32_fmc2_match[] = {
+	{ .compatible = "st,stm32mp15-fmc2" },
+	{ /* Sentinel */ }
+};
+
+U_BOOT_DRIVER(stm32_fmc2_nand) = {
+	.name = "stm32_fmc2_nand",
+	.id = UCLASS_MTD,
+	.of_match = stm32_fmc2_match,
+	.probe = stm32_fmc2_probe,
+	.priv_auto_alloc_size = sizeof(struct stm32_fmc2_nfc),
+};
+
+void board_nand_init(void)
+{
+	struct udevice *dev;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_MTD,
+					  DM_GET_DRIVER(stm32_fmc2_nand),
+					  &dev);
+	if (ret && ret != -ENODEV)
+		pr_err("Failed to initialize STM32 FMC2 NAND controller. (error %d)\n",
+		       ret);
+}
diff --git a/drivers/mtd/spi/spi_flash.c b/drivers/mtd/spi/spi_flash.c
index a87bacd..3bd6869 100644
--- a/drivers/mtd/spi/spi_flash.c
+++ b/drivers/mtd/spi/spi_flash.c
@@ -14,6 +14,7 @@
 #include <mapmem.h>
 #include <spi.h>
 #include <spi_flash.h>
+#include <watchdog.h>
 #include <linux/log2.h>
 #include <linux/sizes.h>
 #include <dma.h>
@@ -357,6 +358,8 @@ int spi_flash_cmd_erase_ops(struct spi_flash *flash, u32 offset, size_t len)
 
 		offset += erase_size;
 		len -= erase_size;
+
+		WATCHDOG_RESET();
 	}
 
 #ifdef CONFIG_SPI_FLASH_BAR
@@ -419,6 +422,8 @@ int spi_flash_cmd_write_ops(struct spi_flash *flash, u32 offset,
 		}
 
 		offset += chunk_len;
+
+		WATCHDOG_RESET();
 	}
 
 #ifdef CONFIG_SPI_FLASH_BAR
@@ -525,6 +530,8 @@ int spi_flash_cmd_read_ops(struct spi_flash *flash, u32 offset,
 		offset += read_len;
 		len -= read_len;
 		data += read_len;
+
+		WATCHDOG_RESET();
 	}
 
 #ifdef CONFIG_SPI_FLASH_BAR
@@ -769,6 +776,8 @@ int sst_write_wp(struct spi_flash *flash, u32 offset, size_t len,
 
 		cmd_len = 1;
 		offset += 2;
+
+		WATCHDOG_RESET();
 	}
 
 	if (!ret)
diff --git a/drivers/net/dwc_eth_qos.c b/drivers/net/dwc_eth_qos.c
index 9f1c5af..072b8c8 100644
--- a/drivers/net/dwc_eth_qos.c
+++ b/drivers/net/dwc_eth_qos.c
@@ -26,7 +26,6 @@
  *    supports a single RGMII PHY. This configuration also has SW control over
  *    all clock and reset signals to the HW block.
  */
-
 #include <common.h>
 #include <clk.h>
 #include <dm.h>
@@ -95,6 +94,7 @@ struct eqos_mac_regs {
 #define EQOS_MAC_RXQ_CTRL0_RXQ0EN_MASK			3
 #define EQOS_MAC_RXQ_CTRL0_RXQ0EN_NOT_ENABLED		0
 #define EQOS_MAC_RXQ_CTRL0_RXQ0EN_ENABLED_DCB		2
+#define EQOS_MAC_RXQ_CTRL0_RXQ0EN_ENABLED_AV		1
 
 #define EQOS_MAC_RXQ_CTRL2_PSRQ0_SHIFT			0
 #define EQOS_MAC_RXQ_CTRL2_PSRQ0_MASK			0xff
@@ -108,6 +108,7 @@ struct eqos_mac_regs {
 #define EQOS_MAC_MDIO_ADDRESS_RDA_SHIFT			16
 #define EQOS_MAC_MDIO_ADDRESS_CR_SHIFT			8
 #define EQOS_MAC_MDIO_ADDRESS_CR_20_35			2
+#define EQOS_MAC_MDIO_ADDRESS_CR_250_300		5
 #define EQOS_MAC_MDIO_ADDRESS_SKAP			BIT(4)
 #define EQOS_MAC_MDIO_ADDRESS_GOC_SHIFT			2
 #define EQOS_MAC_MDIO_ADDRESS_GOC_READ			3
@@ -260,6 +261,28 @@ struct eqos_desc {
 
 struct eqos_config {
 	bool reg_access_always_ok;
+	int mdio_wait;
+	int config_mac;
+	int config_mac_mdio;
+	int (*interface)(struct udevice *);
+	struct eqos_ops *ops;
+};
+
+struct eqos_ops {
+	void (*eqos_inval_desc)(void *desc);
+	void (*eqos_flush_desc)(void *desc);
+	void (*eqos_inval_buffer)(void *buf, size_t size);
+	void (*eqos_flush_buffer)(void *buf, size_t size);
+	int (*eqos_probe_resources)(struct udevice *);
+	int (*eqos_remove_resources)(struct udevice *);
+	int (*eqos_stop_resets)(struct udevice *);
+	int (*eqos_start_resets)(struct udevice *);
+	void (*eqos_stop_clks)(struct udevice *);
+	int (*eqos_start_clks)(struct udevice *);
+	int (*eqos_calibrate_pads)(struct udevice *);
+	int (*eqos_disable_calibration)(struct udevice *);
+	int (*eqos_set_tx_clk_speed)(struct udevice *);
+	ulong (*eqos_get_tick_clk_rate)(struct udevice *);
 };
 
 struct eqos_priv {
@@ -276,6 +299,7 @@ struct eqos_priv {
 	struct clk clk_rx;
 	struct clk clk_ptp_ref;
 	struct clk clk_tx;
+	struct clk clk_ck;
 	struct clk clk_slave_bus;
 	struct mii_dev *mii;
 	struct phy_device *phy;
@@ -327,7 +351,7 @@ static void eqos_free_descs(void *descs)
 #endif
 }
 
-static void eqos_inval_desc(void *desc)
+static void eqos_inval_desc_tegra186(void *desc)
 {
 #ifndef CONFIG_SYS_NONCACHED_MEMORY
 	unsigned long start = (unsigned long)desc & ~(ARCH_DMA_MINALIGN - 1);
@@ -338,14 +362,36 @@ static void eqos_inval_desc(void *desc)
 #endif
 }
 
-static void eqos_flush_desc(void *desc)
+static void eqos_inval_desc_stm32(void *desc)
+{
+#ifndef CONFIG_SYS_NONCACHED_MEMORY
+	unsigned long start = rounddown((unsigned long)desc, ARCH_DMA_MINALIGN);
+	unsigned long end = roundup((unsigned long)desc + EQOS_DESCRIPTOR_SIZE,
+				    ARCH_DMA_MINALIGN);
+
+	invalidate_dcache_range(start, end);
+#endif
+}
+
+static void eqos_flush_desc_tegra186(void *desc)
 {
 #ifndef CONFIG_SYS_NONCACHED_MEMORY
 	flush_cache((unsigned long)desc, EQOS_DESCRIPTOR_SIZE);
 #endif
 }
 
-static void eqos_inval_buffer(void *buf, size_t size)
+static void eqos_flush_desc_stm32(void *desc)
+{
+#ifndef CONFIG_SYS_NONCACHED_MEMORY
+	unsigned long start = rounddown((unsigned long)desc, ARCH_DMA_MINALIGN);
+	unsigned long end = roundup((unsigned long)desc + EQOS_DESCRIPTOR_SIZE,
+				    ARCH_DMA_MINALIGN);
+
+	flush_dcache_range(start, end);
+#endif
+}
+
+static void eqos_inval_buffer_tegra186(void *buf, size_t size)
 {
 	unsigned long start = (unsigned long)buf & ~(ARCH_DMA_MINALIGN - 1);
 	unsigned long end = ALIGN(start + size, ARCH_DMA_MINALIGN);
@@ -353,11 +399,29 @@ static void eqos_inval_buffer(void *buf, size_t size)
 	invalidate_dcache_range(start, end);
 }
 
-static void eqos_flush_buffer(void *buf, size_t size)
+static void eqos_inval_buffer_stm32(void *buf, size_t size)
+{
+	unsigned long start = rounddown((unsigned long)buf, ARCH_DMA_MINALIGN);
+	unsigned long end = roundup((unsigned long)buf + size,
+				    ARCH_DMA_MINALIGN);
+
+	invalidate_dcache_range(start, end);
+}
+
+static void eqos_flush_buffer_tegra186(void *buf, size_t size)
 {
 	flush_cache((unsigned long)buf, size);
 }
 
+static void eqos_flush_buffer_stm32(void *buf, size_t size)
+{
+	unsigned long start = rounddown((unsigned long)buf, ARCH_DMA_MINALIGN);
+	unsigned long end = roundup((unsigned long)buf + size,
+				    ARCH_DMA_MINALIGN);
+
+	flush_dcache_range(start, end);
+}
+
 static int eqos_mdio_wait_idle(struct eqos_priv *eqos)
 {
 	return wait_for_bit_le32(&eqos->mac_regs->mdio_address,
@@ -386,14 +450,14 @@ static int eqos_mdio_read(struct mii_dev *bus, int mdio_addr, int mdio_devad,
 		EQOS_MAC_MDIO_ADDRESS_C45E;
 	val |= (mdio_addr << EQOS_MAC_MDIO_ADDRESS_PA_SHIFT) |
 		(mdio_reg << EQOS_MAC_MDIO_ADDRESS_RDA_SHIFT) |
-		(EQOS_MAC_MDIO_ADDRESS_CR_20_35 <<
+		(eqos->config->config_mac_mdio <<
 		 EQOS_MAC_MDIO_ADDRESS_CR_SHIFT) |
 		(EQOS_MAC_MDIO_ADDRESS_GOC_READ <<
 		 EQOS_MAC_MDIO_ADDRESS_GOC_SHIFT) |
 		EQOS_MAC_MDIO_ADDRESS_GB;
 	writel(val, &eqos->mac_regs->mdio_address);
 
-	udelay(10);
+	udelay(eqos->config->mdio_wait);
 
 	ret = eqos_mdio_wait_idle(eqos);
 	if (ret) {
@@ -432,14 +496,14 @@ static int eqos_mdio_write(struct mii_dev *bus, int mdio_addr, int mdio_devad,
 		EQOS_MAC_MDIO_ADDRESS_C45E;
 	val |= (mdio_addr << EQOS_MAC_MDIO_ADDRESS_PA_SHIFT) |
 		(mdio_reg << EQOS_MAC_MDIO_ADDRESS_RDA_SHIFT) |
-		(EQOS_MAC_MDIO_ADDRESS_CR_20_35 <<
+		(eqos->config->config_mac_mdio <<
 		 EQOS_MAC_MDIO_ADDRESS_CR_SHIFT) |
 		(EQOS_MAC_MDIO_ADDRESS_GOC_WRITE <<
 		 EQOS_MAC_MDIO_ADDRESS_GOC_SHIFT) |
 		EQOS_MAC_MDIO_ADDRESS_GB;
 	writel(val, &eqos->mac_regs->mdio_address);
 
-	udelay(10);
+	udelay(eqos->config->mdio_wait);
 
 	ret = eqos_mdio_wait_idle(eqos);
 	if (ret) {
@@ -509,6 +573,53 @@ err:
 	return ret;
 }
 
+static int eqos_start_clks_stm32(struct udevice *dev)
+{
+	struct eqos_priv *eqos = dev_get_priv(dev);
+	int ret;
+
+	debug("%s(dev=%p):\n", __func__, dev);
+
+	ret = clk_enable(&eqos->clk_master_bus);
+	if (ret < 0) {
+		pr_err("clk_enable(clk_master_bus) failed: %d", ret);
+		goto err;
+	}
+
+	ret = clk_enable(&eqos->clk_rx);
+	if (ret < 0) {
+		pr_err("clk_enable(clk_rx) failed: %d", ret);
+		goto err_disable_clk_master_bus;
+	}
+
+	ret = clk_enable(&eqos->clk_tx);
+	if (ret < 0) {
+		pr_err("clk_enable(clk_tx) failed: %d", ret);
+		goto err_disable_clk_rx;
+	}
+
+	if (clk_valid(&eqos->clk_ck)) {
+		ret = clk_enable(&eqos->clk_ck);
+		if (ret < 0) {
+			pr_err("clk_enable(clk_ck) failed: %d", ret);
+			goto err_disable_clk_tx;
+		}
+	}
+
+	debug("%s: OK\n", __func__);
+	return 0;
+
+err_disable_clk_tx:
+	clk_disable(&eqos->clk_tx);
+err_disable_clk_rx:
+	clk_disable(&eqos->clk_rx);
+err_disable_clk_master_bus:
+	clk_disable(&eqos->clk_master_bus);
+err:
+	debug("%s: FAILED: %d\n", __func__, ret);
+	return ret;
+}
+
 void eqos_stop_clks_tegra186(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -524,6 +635,21 @@ void eqos_stop_clks_tegra186(struct udevice *dev)
 	debug("%s: OK\n", __func__);
 }
 
+void eqos_stop_clks_stm32(struct udevice *dev)
+{
+	struct eqos_priv *eqos = dev_get_priv(dev);
+
+	debug("%s(dev=%p):\n", __func__, dev);
+
+	clk_disable(&eqos->clk_tx);
+	clk_disable(&eqos->clk_rx);
+	clk_disable(&eqos->clk_master_bus);
+	if (clk_valid(&eqos->clk_ck))
+		clk_disable(&eqos->clk_ck);
+
+	debug("%s: OK\n", __func__);
+}
+
 static int eqos_start_resets_tegra186(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -563,6 +689,11 @@ static int eqos_start_resets_tegra186(struct udevice *dev)
 	return 0;
 }
 
+static int eqos_start_resets_stm32(struct udevice *dev)
+{
+	return 0;
+}
+
 static int eqos_stop_resets_tegra186(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -573,6 +704,11 @@ static int eqos_stop_resets_tegra186(struct udevice *dev)
 	return 0;
 }
 
+static int eqos_stop_resets_stm32(struct udevice *dev)
+{
+	return 0;
+}
+
 static int eqos_calibrate_pads_tegra186(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -632,6 +768,23 @@ static ulong eqos_get_tick_clk_rate_tegra186(struct udevice *dev)
 	return clk_get_rate(&eqos->clk_slave_bus);
 }
 
+static ulong eqos_get_tick_clk_rate_stm32(struct udevice *dev)
+{
+	struct eqos_priv *eqos = dev_get_priv(dev);
+
+	return clk_get_rate(&eqos->clk_master_bus);
+}
+
+static int eqos_calibrate_pads_stm32(struct udevice *dev)
+{
+	return 0;
+}
+
+static int eqos_disable_calibration_stm32(struct udevice *dev)
+{
+	return 0;
+}
+
 static int eqos_set_full_duplex(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -726,6 +879,11 @@ static int eqos_set_tx_clk_speed_tegra186(struct udevice *dev)
 	return 0;
 }
 
+static int eqos_set_tx_clk_speed_stm32(struct udevice *dev)
+{
+	return 0;
+}
+
 static int eqos_adjust_link(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -766,23 +924,23 @@ static int eqos_adjust_link(struct udevice *dev)
 	}
 
 	if (en_calibration) {
-		ret = eqos_calibrate_pads_tegra186(dev);
+		ret = eqos->config->ops->eqos_calibrate_pads(dev);
 		if (ret < 0) {
-			pr_err("eqos_calibrate_pads_tegra186() failed: %d", ret);
+			pr_err("eqos_calibrate_pads() failed: %d",
+			       ret);
 			return ret;
 		}
 	} else {
-		ret = eqos_disable_calibration_tegra186(dev);
+		ret = eqos->config->ops->eqos_disable_calibration(dev);
 		if (ret < 0) {
-			pr_err("eqos_disable_calibration_tegra186() failed: %d",
-			      ret);
+			pr_err("eqos_disable_calibration() failed: %d",
+			       ret);
 			return ret;
 		}
 	}
-
-	ret = eqos_set_tx_clk_speed_tegra186(dev);
+	ret = eqos->config->ops->eqos_set_tx_clk_speed(dev);
 	if (ret < 0) {
-		pr_err("eqos_set_tx_clk_speed_tegra186() failed: %d", ret);
+		pr_err("eqos_set_tx_clk_speed() failed: %d", ret);
 		return ret;
 	}
 
@@ -846,18 +1004,12 @@ static int eqos_start(struct udevice *dev)
 	eqos->tx_desc_idx = 0;
 	eqos->rx_desc_idx = 0;
 
-	ret = eqos_start_clks_tegra186(dev);
+	ret = eqos->config->ops->eqos_start_resets(dev);
 	if (ret < 0) {
-		pr_err("eqos_start_clks_tegra186() failed: %d", ret);
+		pr_err("eqos_start_resets() failed: %d", ret);
 		goto err;
 	}
 
-	ret = eqos_start_resets_tegra186(dev);
-	if (ret < 0) {
-		pr_err("eqos_start_resets_tegra186() failed: %d", ret);
-		goto err_stop_clks;
-	}
-
 	udelay(10);
 
 	eqos->reg_access_ok = true;
@@ -869,26 +1021,16 @@ static int eqos_start(struct udevice *dev)
 		goto err_stop_resets;
 	}
 
-	ret = eqos_calibrate_pads_tegra186(dev);
+	ret = eqos->config->ops->eqos_calibrate_pads(dev);
 	if (ret < 0) {
-		pr_err("eqos_calibrate_pads_tegra186() failed: %d", ret);
+		pr_err("eqos_calibrate_pads() failed: %d", ret);
 		goto err_stop_resets;
 	}
+	rate = eqos->config->ops->eqos_get_tick_clk_rate(dev);
 
-	rate = eqos_get_tick_clk_rate_tegra186(dev);
 	val = (rate / 1000000) - 1;
 	writel(val, &eqos->mac_regs->us_tic_counter);
 
-	eqos->phy = phy_connect(eqos->mii, 0, dev, 0);
-	if (!eqos->phy) {
-		pr_err("phy_connect() failed");
-		goto err_stop_resets;
-	}
-	ret = phy_config(eqos->phy);
-	if (ret < 0) {
-		pr_err("phy_config() failed: %d", ret);
-		goto err_shutdown_phy;
-	}
 	ret = phy_startup(eqos->phy);
 	if (ret < 0) {
 		pr_err("phy_startup() failed: %d", ret);
@@ -993,7 +1135,7 @@ static int eqos_start(struct udevice *dev)
 	clrsetbits_le32(&eqos->mac_regs->rxq_ctrl0,
 			EQOS_MAC_RXQ_CTRL0_RXQ0EN_MASK <<
 			EQOS_MAC_RXQ_CTRL0_RXQ0EN_SHIFT,
-			EQOS_MAC_RXQ_CTRL0_RXQ0EN_ENABLED_DCB <<
+			eqos->config->config_mac <<
 			EQOS_MAC_RXQ_CTRL0_RXQ0EN_SHIFT);
 
 	/* Set TX flow control parameters */
@@ -1074,7 +1216,7 @@ static int eqos_start(struct udevice *dev)
 					     (i * EQOS_MAX_PACKET_SIZE));
 		rx_desc->des3 |= EQOS_DESC3_OWN | EQOS_DESC3_BUF1V;
 	}
-	flush_cache((unsigned long)eqos->descs, EQOS_DESCRIPTORS_SIZE);
+	eqos->config->ops->eqos_flush_desc(eqos->descs);
 
 	writel(0, &eqos->dma_regs->ch0_txdesc_list_haddress);
 	writel((ulong)eqos->tx_descs, &eqos->dma_regs->ch0_txdesc_list_address);
@@ -1115,9 +1257,7 @@ err_shutdown_phy:
 	phy_shutdown(eqos->phy);
 	eqos->phy = NULL;
 err_stop_resets:
-	eqos_stop_resets_tegra186(dev);
-err_stop_clks:
-	eqos_stop_clks_tegra186(dev);
+	eqos->config->ops->eqos_stop_resets(dev);
 err:
 	pr_err("FAILED: %d", ret);
 	return ret;
@@ -1168,12 +1308,7 @@ void eqos_stop(struct udevice *dev)
 	clrbits_le32(&eqos->dma_regs->ch0_rx_control,
 		     EQOS_DMA_CH0_RX_CONTROL_SR);
 
-	if (eqos->phy) {
-		phy_shutdown(eqos->phy);
-		eqos->phy = NULL;
-	}
-	eqos_stop_resets_tegra186(dev);
-	eqos_stop_clks_tegra186(dev);
+	eqos->config->ops->eqos_stop_resets(dev);
 
 	debug("%s: OK\n", __func__);
 }
@@ -1188,7 +1323,7 @@ int eqos_send(struct udevice *dev, void *packet, int length)
 	      length);
 
 	memcpy(eqos->tx_dma_buf, packet, length);
-	eqos_flush_buffer(eqos->tx_dma_buf, length);
+	eqos->config->ops->eqos_flush_buffer(eqos->tx_dma_buf, length);
 
 	tx_desc = &(eqos->tx_descs[eqos->tx_desc_idx]);
 	eqos->tx_desc_idx++;
@@ -1203,12 +1338,12 @@ int eqos_send(struct udevice *dev, void *packet, int length)
 	 */
 	mb();
 	tx_desc->des3 = EQOS_DESC3_OWN | EQOS_DESC3_FD | EQOS_DESC3_LD | length;
-	eqos_flush_desc(tx_desc);
+	eqos->config->ops->eqos_flush_desc(tx_desc);
 
 	writel((ulong)(tx_desc + 1), &eqos->dma_regs->ch0_txdesc_tail_pointer);
 
 	for (i = 0; i < 1000000; i++) {
-		eqos_inval_desc(tx_desc);
+		eqos->config->ops->eqos_inval_desc(tx_desc);
 		if (!(readl(&tx_desc->des3) & EQOS_DESC3_OWN))
 			return 0;
 		udelay(1);
@@ -1238,7 +1373,7 @@ int eqos_recv(struct udevice *dev, int flags, uchar **packetp)
 	length = rx_desc->des3 & 0x7fff;
 	debug("%s: *packetp=%p, length=%d\n", __func__, *packetp, length);
 
-	eqos_inval_buffer(*packetp, length);
+	eqos->config->ops->eqos_inval_buffer(*packetp, length);
 
 	return length;
 }
@@ -1269,7 +1404,7 @@ int eqos_free_pkt(struct udevice *dev, uchar *packet, int length)
 	 */
 	mb();
 	rx_desc->des3 |= EQOS_DESC3_OWN | EQOS_DESC3_BUF1V;
-	eqos_flush_desc(rx_desc);
+	eqos->config->ops->eqos_flush_desc(rx_desc);
 
 	writel((ulong)rx_desc, &eqos->dma_regs->ch0_rxdesc_tail_pointer);
 
@@ -1304,7 +1439,7 @@ static int eqos_probe_resources_core(struct udevice *dev)
 		ret = -ENOMEM;
 		goto err_free_descs;
 	}
-	debug("%s: rx_dma_buf=%p\n", __func__, eqos->rx_dma_buf);
+	debug("%s: tx_dma_buf=%p\n", __func__, eqos->tx_dma_buf);
 
 	eqos->rx_dma_buf = memalign(EQOS_BUFFER_ALIGN, EQOS_RX_BUFFER_SIZE);
 	if (!eqos->rx_dma_buf) {
@@ -1312,7 +1447,7 @@ static int eqos_probe_resources_core(struct udevice *dev)
 		ret = -ENOMEM;
 		goto err_free_tx_dma_buf;
 	}
-	debug("%s: tx_dma_buf=%p\n", __func__, eqos->tx_dma_buf);
+	debug("%s: rx_dma_buf=%p\n", __func__, eqos->rx_dma_buf);
 
 	eqos->rx_pkt = malloc(EQOS_MAX_PACKET_SIZE);
 	if (!eqos->rx_pkt) {
@@ -1424,6 +1559,99 @@ err_free_reset_eqos:
 	return ret;
 }
 
+/* board-specific Ethernet Interface initializations. */
+__weak int board_interface_eth_init(int interface_type, bool eth_clk_sel_reg,
+				    bool eth_ref_clk_sel_reg)
+{
+	return 0;
+}
+
+static int eqos_probe_resources_stm32(struct udevice *dev)
+{
+	struct eqos_priv *eqos = dev_get_priv(dev);
+	int ret;
+	int interface;
+	bool eth_clk_sel_reg = false;
+	bool eth_ref_clk_sel_reg = false;
+
+
+	debug("%s(dev=%p):\n", __func__, dev);
+
+	interface = eqos->config->interface(dev);
+
+	if (interface == -1) {
+		pr_err("Invalid PHY interface\n");
+		return -EINVAL;
+	}
+
+	/* Gigabit Ethernet 125MHz clock selection. */
+	eth_clk_sel_reg = dev_read_bool(dev, "st,eth_clk_sel");
+
+	/* Ethernet 50Mhz RMII clock selection */
+	eth_ref_clk_sel_reg =
+		dev_read_bool(dev, "st,eth_ref_clk_sel");
+
+	ret = board_interface_eth_init(interface, eth_clk_sel_reg,
+				       eth_ref_clk_sel_reg);
+	if (ret)
+		return -EINVAL;
+
+	ret = clk_get_by_name(dev, "stmmaceth", &eqos->clk_master_bus);
+	if (ret) {
+		pr_err("clk_get_by_name(master_bus) failed: %d", ret);
+		goto err_probe;
+	}
+
+	ret = clk_get_by_name(dev, "mac-clk-rx", &eqos->clk_rx);
+	if (ret) {
+		pr_err("clk_get_by_name(rx) failed: %d", ret);
+		goto err_free_clk_master_bus;
+	}
+
+	ret = clk_get_by_name(dev, "mac-clk-tx", &eqos->clk_tx);
+	if (ret) {
+		pr_err("clk_get_by_name(tx) failed: %d", ret);
+		goto err_free_clk_rx;
+	}
+
+	/*  Get ETH_CLK clocks (optional) */
+	ret = clk_get_by_name(dev, "eth-ck", &eqos->clk_ck);
+	if (ret)
+		pr_warn("No phy clock provided %d", ret);
+
+	debug("%s: OK\n", __func__);
+	return 0;
+
+err_free_clk_rx:
+	clk_free(&eqos->clk_rx);
+err_free_clk_master_bus:
+	clk_free(&eqos->clk_master_bus);
+err_probe:
+
+	debug("%s: returns %d\n", __func__, ret);
+	return ret;
+}
+
+static int eqos_get_interface_stm32(struct udevice *dev)
+{
+	const char *phy_mode;
+	int interface = -1;
+
+	debug("%s(dev=%p):\n", __func__, dev);
+
+	phy_mode = fdt_getprop(gd->fdt_blob, dev_of_offset(dev), "phy-mode",
+			       NULL);
+	if (phy_mode)
+		interface = phy_get_interface_by_name(phy_mode);
+
+	return interface;
+}
+
+static int eqos_get_interface_tegra186(struct udevice *dev)
+{
+	return 0;
+}
+
 static int eqos_remove_resources_tegra186(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -1442,6 +1670,22 @@ static int eqos_remove_resources_tegra186(struct udevice *dev)
 	return 0;
 }
 
+static int eqos_remove_resources_stm32(struct udevice *dev)
+{
+	struct eqos_priv *eqos = dev_get_priv(dev);
+
+	debug("%s(dev=%p):\n", __func__, dev);
+
+	clk_free(&eqos->clk_tx);
+	clk_free(&eqos->clk_rx);
+	clk_free(&eqos->clk_master_bus);
+	if (clk_valid(&eqos->clk_ck))
+		clk_free(&eqos->clk_ck);
+
+	debug("%s: OK\n", __func__);
+	return 0;
+}
+
 static int eqos_probe(struct udevice *dev)
 {
 	struct eqos_priv *eqos = dev_get_priv(dev);
@@ -1468,9 +1712,9 @@ static int eqos_probe(struct udevice *dev)
 		return ret;
 	}
 
-	ret = eqos_probe_resources_tegra186(dev);
+	ret = eqos->config->ops->eqos_probe_resources(dev);
 	if (ret < 0) {
-		pr_err("eqos_probe_resources_tegra186() failed: %d", ret);
+		pr_err("eqos_probe_resources() failed: %d", ret);
 		goto err_remove_resources_core;
 	}
 
@@ -1490,13 +1734,38 @@ static int eqos_probe(struct udevice *dev)
 		goto err_free_mdio;
 	}
 
+	// Bring up PHY
+	ret = eqos->config->ops->eqos_start_clks(dev);
+	if (ret < 0) {
+		pr_err("eqos_start_clks() failed: %d", ret);
+		goto err_free_mdio;
+	}
+
+	eqos->phy = phy_connect(eqos->mii, 0, dev,
+				eqos->config->interface(dev));
+	if (!eqos->phy) {
+		pr_err("phy_connect() failed");
+		goto err_stop_resets;
+	}
+	ret = phy_config(eqos->phy);
+	if (ret < 0) {
+		pr_err("phy_config() failed: %d", ret);
+		goto err_shutdown_phy;
+	}
+
 	debug("%s: OK\n", __func__);
 	return 0;
 
+err_shutdown_phy:
+	phy_shutdown(eqos->phy);
+	eqos->phy = NULL;
+err_stop_resets:
+	eqos->config->ops->eqos_stop_resets(dev);
+	eqos->config->ops->eqos_stop_clks(dev);
 err_free_mdio:
 	mdio_free(eqos->mii);
 err_remove_resources_tegra:
-	eqos_remove_resources_tegra186(dev);
+	eqos->config->ops->eqos_remove_resources(dev);
 err_remove_resources_core:
 	eqos_remove_resources_core(dev);
 
@@ -1512,7 +1781,16 @@ static int eqos_remove(struct udevice *dev)
 
 	mdio_unregister(eqos->mii);
 	mdio_free(eqos->mii);
-	eqos_remove_resources_tegra186(dev);
+
+	if (eqos->phy) {
+		phy_shutdown(eqos->phy);
+		eqos->phy = NULL;
+	}
+
+	eqos->config->ops->eqos_stop_resets(dev);
+	eqos->config->ops->eqos_stop_clks(dev);
+	eqos->config->ops->eqos_remove_resources(dev);
+
 	eqos_probe_resources_core(dev);
 
 	debug("%s: OK\n", __func__);
@@ -1528,8 +1806,56 @@ static const struct eth_ops eqos_ops = {
 	.write_hwaddr = eqos_write_hwaddr,
 };
 
+static struct eqos_ops eqos_tegra186_ops = {
+	.eqos_inval_desc = eqos_inval_desc_tegra186,
+	.eqos_flush_desc = eqos_flush_desc_tegra186,
+	.eqos_inval_buffer = eqos_inval_buffer_tegra186,
+	.eqos_flush_buffer = eqos_flush_buffer_tegra186,
+	.eqos_probe_resources = eqos_probe_resources_tegra186,
+	.eqos_remove_resources = eqos_remove_resources_tegra186,
+	.eqos_stop_resets = eqos_stop_resets_tegra186,
+	.eqos_start_resets = eqos_start_resets_tegra186,
+	.eqos_stop_clks = eqos_stop_clks_tegra186,
+	.eqos_start_clks = eqos_start_clks_tegra186,
+	.eqos_calibrate_pads = eqos_calibrate_pads_tegra186,
+	.eqos_disable_calibration = eqos_disable_calibration_tegra186,
+	.eqos_set_tx_clk_speed = eqos_set_tx_clk_speed_tegra186,
+	.eqos_get_tick_clk_rate = eqos_get_tick_clk_rate_tegra186
+};
+
 static const struct eqos_config eqos_tegra186_config = {
 	.reg_access_always_ok = false,
+	.mdio_wait = 10,
+	.config_mac = EQOS_MAC_RXQ_CTRL0_RXQ0EN_ENABLED_DCB,
+	.config_mac_mdio = EQOS_MAC_MDIO_ADDRESS_CR_20_35,
+	.interface = eqos_get_interface_tegra186,
+	.ops = &eqos_tegra186_ops
+};
+
+static struct eqos_ops eqos_stm32_ops = {
+	.eqos_inval_desc = eqos_inval_desc_stm32,
+	.eqos_flush_desc = eqos_flush_desc_stm32,
+	.eqos_inval_buffer = eqos_inval_buffer_stm32,
+	.eqos_flush_buffer = eqos_flush_buffer_stm32,
+	.eqos_probe_resources = eqos_probe_resources_stm32,
+	.eqos_remove_resources = eqos_remove_resources_stm32,
+	.eqos_stop_resets = eqos_stop_resets_stm32,
+	.eqos_start_resets = eqos_start_resets_stm32,
+	.eqos_stop_clks = eqos_stop_clks_stm32,
+	.eqos_start_clks = eqos_start_clks_stm32,
+	.eqos_calibrate_pads = eqos_calibrate_pads_stm32,
+	.eqos_disable_calibration = eqos_disable_calibration_stm32,
+	.eqos_set_tx_clk_speed = eqos_set_tx_clk_speed_stm32,
+	.eqos_get_tick_clk_rate = eqos_get_tick_clk_rate_stm32
+};
+
+static const struct eqos_config eqos_stm32_config = {
+	.reg_access_always_ok = false,
+	.mdio_wait = 10000,
+	.config_mac = EQOS_MAC_RXQ_CTRL0_RXQ0EN_ENABLED_AV,
+	.config_mac_mdio = EQOS_MAC_MDIO_ADDRESS_CR_250_300,
+	.interface = eqos_get_interface_stm32,
+	.ops = &eqos_stm32_ops
 };
 
 static const struct udevice_id eqos_ids[] = {
@@ -1537,6 +1863,11 @@ static const struct udevice_id eqos_ids[] = {
 		.compatible = "nvidia,tegra186-eqos",
 		.data = (ulong)&eqos_tegra186_config
 	},
+	{
+		.compatible = "snps,dwmac-4.20a",
+		.data = (ulong)&eqos_stm32_config
+	},
+
 	{ }
 };
 
diff --git a/drivers/phy/phy-stm32-usbphyc.c b/drivers/phy/phy-stm32-usbphyc.c
index 8e98b4b..22937d8 100644
--- a/drivers/phy/phy-stm32-usbphyc.c
+++ b/drivers/phy/phy-stm32-usbphyc.c
@@ -37,7 +37,8 @@
 
 #define MAX_PHYS		2
 
-#define PLL_LOCK_TIME_US	100
+/* max 100 us for PLL lock and 100 us for PHY init */
+#define PLL_INIT_TIME_US	200
 #define PLL_PWR_DOWN_TIME_US	5
 #define PLL_FVCO		2880	 /* in MHz */
 #define PLL_INFF_MIN_RATE	19200000 /* in Hz */
@@ -51,13 +52,11 @@ struct pll_params {
 struct stm32_usbphyc {
 	fdt_addr_t base;
 	struct clk clk;
+	struct udevice *vdda1v1;
+	struct udevice *vdda1v8;
+	struct udevice *vdd3v3;
 	struct stm32_usbphyc_phy {
-		struct udevice *vdd;
-		struct udevice *vdda1v1;
-		struct udevice *vdda1v8;
-		int index;
 		bool init;
-		bool powered;
 	} phys[MAX_PHYS];
 };
 
@@ -129,18 +128,6 @@ static bool stm32_usbphyc_is_init(struct stm32_usbphyc *usbphyc)
 	return false;
 }
 
-static bool stm32_usbphyc_is_powered(struct stm32_usbphyc *usbphyc)
-{
-	int i;
-
-	for (i = 0; i < MAX_PHYS; i++) {
-		if (usbphyc->phys[i].powered)
-			return true;
-	}
-
-	return false;
-}
-
 static int stm32_usbphyc_phy_init(struct phy *phy)
 {
 	struct stm32_usbphyc *usbphyc = dev_get_priv(phy->dev);
@@ -154,6 +141,24 @@ static int stm32_usbphyc_phy_init(struct phy *phy)
 	if (pllen && stm32_usbphyc_is_init(usbphyc))
 		goto initialized;
 
+	if (usbphyc->vdda1v1) {
+		ret = regulator_set_enable(usbphyc->vdda1v1, true);
+		if (ret)
+			return ret;
+	}
+
+	if (usbphyc->vdda1v8) {
+		ret = regulator_set_enable(usbphyc->vdda1v8, true);
+		if (ret)
+			return ret;
+	}
+
+	if (usbphyc->vdd3v3) {
+		ret = regulator_set_enable(usbphyc->vdd3v3, true);
+		if (ret)
+			return ret;
+	}
+
 	if (pllen) {
 		clrbits_le32(usbphyc->base + STM32_USBPHYC_PLL, PLLEN);
 		udelay(PLL_PWR_DOWN_TIME_US);
@@ -165,11 +170,8 @@ static int stm32_usbphyc_phy_init(struct phy *phy)
 
 	setbits_le32(usbphyc->base + STM32_USBPHYC_PLL, PLLEN);
 
-	/*
-	 * We must wait PLL_LOCK_TIME_US before checking that PLLEN
-	 * bit is still set
-	 */
-	udelay(PLL_LOCK_TIME_US);
+	/* We must wait PLL_INIT_TIME_US before using PHY */
+	udelay(PLL_INIT_TIME_US);
 
 	if (!(readl(usbphyc->base + STM32_USBPHYC_PLL) & PLLEN))
 		return -EIO;
@@ -184,6 +186,7 @@ static int stm32_usbphyc_phy_exit(struct phy *phy)
 {
 	struct stm32_usbphyc *usbphyc = dev_get_priv(phy->dev);
 	struct stm32_usbphyc_phy *usbphyc_phy = usbphyc->phys + phy->id;
+	int ret;
 
 	pr_debug("%s phy ID = %lu\n", __func__, phy->id);
 	usbphyc_phy->init = false;
@@ -202,65 +205,20 @@ static int stm32_usbphyc_phy_exit(struct phy *phy)
 
 	if (readl(usbphyc->base + STM32_USBPHYC_PLL) & PLLEN)
 		return -EIO;
-
-	return 0;
-}
-
-static int stm32_usbphyc_phy_power_on(struct phy *phy)
-{
-	struct stm32_usbphyc *usbphyc = dev_get_priv(phy->dev);
-	struct stm32_usbphyc_phy *usbphyc_phy = usbphyc->phys + phy->id;
-	int ret;
-
-	pr_debug("%s phy ID = %lu\n", __func__, phy->id);
-	if (usbphyc_phy->vdda1v1) {
-		ret = regulator_set_enable(usbphyc_phy->vdda1v1, true);
-		if (ret)
-			return ret;
-	}
-
-	if (usbphyc_phy->vdda1v8) {
-		ret = regulator_set_enable(usbphyc_phy->vdda1v8, true);
-		if (ret)
-			return ret;
-	}
-	if (usbphyc_phy->vdd) {
-		ret = regulator_set_enable(usbphyc_phy->vdd, true);
-		if (ret)
-			return ret;
-	}
-
-	usbphyc_phy->powered = true;
-
-	return 0;
-}
-
-static int stm32_usbphyc_phy_power_off(struct phy *phy)
-{
-	struct stm32_usbphyc *usbphyc = dev_get_priv(phy->dev);
-	struct stm32_usbphyc_phy *usbphyc_phy = usbphyc->phys + phy->id;
-	int ret;
-
-	pr_debug("%s phy ID = %lu\n", __func__, phy->id);
-	usbphyc_phy->powered = false;
-
-	if (stm32_usbphyc_is_powered(usbphyc))
-		return 0;
-
-	if (usbphyc_phy->vdda1v1) {
-		ret = regulator_set_enable(usbphyc_phy->vdda1v1, false);
+	if (usbphyc->vdda1v1) {
+		ret = regulator_set_enable(usbphyc->vdda1v1, false);
 		if (ret)
 			return ret;
 	}
 
-	if (usbphyc_phy->vdda1v8) {
-		ret = regulator_set_enable(usbphyc_phy->vdda1v8, false);
+	if (usbphyc->vdda1v8) {
+		ret = regulator_set_enable(usbphyc->vdda1v8, false);
 		if (ret)
 			return ret;
 	}
 
-	if (usbphyc_phy->vdd) {
-		ret = regulator_set_enable(usbphyc_phy->vdd, false);
+	if (usbphyc->vdd3v3) {
+		ret = regulator_set_enable(usbphyc->vdd3v3, false);
 		if (ret)
 			return ret;
 	}
@@ -268,49 +226,23 @@ static int stm32_usbphyc_phy_power_off(struct phy *phy)
 	return 0;
 }
 
-static int stm32_usbphyc_get_regulator(struct udevice *dev, ofnode node,
-				       char *supply_name,
-				       struct udevice **regulator)
-{
-	struct ofnode_phandle_args regulator_phandle;
-	int ret;
-
-	ret = ofnode_parse_phandle_with_args(node, supply_name,
-					     NULL, 0, 0,
-					     &regulator_phandle);
-	if (ret) {
-		dev_err(dev, "Can't find %s property (%d)\n", supply_name, ret);
-		return ret;
-	}
-
-	ret = uclass_get_device_by_ofnode(UCLASS_REGULATOR,
-					  regulator_phandle.node,
-					  regulator);
-
-	if (ret) {
-		dev_err(dev, "Can't get %s regulator (%d)\n", supply_name, ret);
-		return ret;
-	}
-
-	return 0;
-}
-
 static int stm32_usbphyc_of_xlate(struct phy *phy,
 				  struct ofnode_phandle_args *args)
 {
-	if (args->args_count > 1) {
-		pr_debug("%s: invalid args_count: %d\n", __func__,
-			 args->args_count);
-		return -EINVAL;
-	}
+	if (args->args_count < 1)
+		return -ENODEV;
 
 	if (args->args[0] >= MAX_PHYS)
 		return -ENODEV;
 
-	if (args->args_count)
-		phy->id = args->args[0];
-	else
-		phy->id = 0;
+	phy->id = args->args[0];
+
+	if ((phy->id == 0 && args->args_count != 1) ||
+	    (phy->id == 1 && args->args_count != 2)) {
+		dev_err(dev, "invalid number of cells for phy port%ld\n",
+			phy->id);
+		return -EINVAL;
+	}
 
 	return 0;
 }
@@ -318,8 +250,6 @@ static int stm32_usbphyc_of_xlate(struct phy *phy,
 static const struct phy_ops stm32_usbphyc_phy_ops = {
 	.init = stm32_usbphyc_phy_init,
 	.exit = stm32_usbphyc_phy_exit,
-	.power_on = stm32_usbphyc_phy_power_on,
-	.power_off = stm32_usbphyc_phy_power_off,
 	.of_xlate = stm32_usbphyc_of_xlate,
 };
 
@@ -327,7 +257,6 @@ static int stm32_usbphyc_probe(struct udevice *dev)
 {
 	struct stm32_usbphyc *usbphyc = dev_get_priv(dev);
 	struct reset_ctl reset;
-	ofnode node;
 	int i, ret;
 
 	usbphyc->base = dev_read_addr(dev);
@@ -351,35 +280,31 @@ static int stm32_usbphyc_probe(struct udevice *dev)
 		reset_deassert(&reset);
 	}
 
-	/*
-	 * parse all PHY subnodes in order to populate regulator associated
-	 * to each PHY port
-	 */
-	node = dev_read_first_subnode(dev);
-	for (i = 0; i < MAX_PHYS; i++) {
-		struct stm32_usbphyc_phy *usbphyc_phy = usbphyc->phys + i;
-
-		usbphyc_phy->index = i;
-		usbphyc_phy->init = false;
-		usbphyc_phy->powered = false;
-		ret = stm32_usbphyc_get_regulator(dev, node, "phy-supply",
-						  &usbphyc_phy->vdd);
-		if (ret)
-			return ret;
-
-		ret = stm32_usbphyc_get_regulator(dev, node, "vdda1v1-supply",
-						  &usbphyc_phy->vdda1v1);
-		if (ret)
-			return ret;
+	/* get usbphyc regulator */
+	ret = device_get_supply_regulator(dev, "vdda1v1-supply",
+					  &usbphyc->vdda1v1);
+	if (ret) {
+		dev_err(dev, "Can't get vdda1v1-supply regulator\n");
+		return ret;
+	}
 
-		ret = stm32_usbphyc_get_regulator(dev, node, "vdda1v8-supply",
-						  &usbphyc_phy->vdda1v8);
-		if (ret)
-			return ret;
+	ret = device_get_supply_regulator(dev, "vdda1v8-supply",
+					  &usbphyc->vdda1v8);
+	if (ret) {
+		dev_err(dev, "Can't get vdda1v8-supply regulator\n");
+		return ret;
+	}
 
-		node = dev_read_next_subnode(node);
+	ret = device_get_supply_regulator(dev, "vdd3v3-supply",
+					  &usbphyc->vdd3v3);
+	if (ret) {
+		dev_err(dev, "Can't get vdd3v3-supply regulator\n");
+		return ret;
 	}
 
+	for (i = 0; i < MAX_PHYS; i++)
+		usbphyc->phys[i].init = false;
+
 	/* Check if second port has to be used for host controller */
 	if (dev_read_bool(dev, "st,port2-switch-to-host"))
 		setbits_le32(usbphyc->base + STM32_USBPHYC_MISC, SWITHOST);
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index ad0b8da..546613d 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -289,6 +289,25 @@ config PINCTRL_STM32
 	  the GPIO definitions and pin control functions for each available
 	  multiplex function.
 
+config PINCTRL_STMFX
+	bool "STMicroelectronics STMFX I2C GPIO expander pinctrl driver"
+	depends on DM && PINCTRL_FULL
+	help
+	  I2C driver for STMicroelectronics Multi-Function eXpander (STMFX)
+	  GPIO expander.
+	  Supports pin multiplexing control on stm32 SoCs.
+
+	  The driver is controlled by a device tree node which contains both
+	  the GPIO definitions and pin control functions for each available
+	  multiplex function.
+
+config SPL_PINCTRL_STMFX
+	bool "STMicroelectronics STMFX I2C GPIO expander pinctrl driver in SPL"
+	depends on SPL_PINCTRL_FULL
+	help
+	  This option is an SPL-variant of the SPL_PINCTRL_STMFX option.
+	  See the help of PINCTRL_STMFX for details.
+
 config ASPEED_AST2500_PINCTRL
   bool "Aspeed AST2500 pin control driver"
   depends on DM && PINCTRL_GENERIC && ASPEED_AST2500
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index a3a6c6d..d2725f6 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -20,4 +20,5 @@ obj-$(CONFIG_ARCH_MVEBU)	+= mvebu/
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_STI)	+= pinctrl-sti.o
 obj-$(CONFIG_PINCTRL_STM32)	+= pinctrl_stm32.o
+obj-$(CONFIG_$(SPL_)PINCTRL_STMFX)	+= pinctrl-stmfx.o
 obj-y				+= broadcom/
diff --git a/drivers/pinctrl/pinctrl-sandbox.c b/drivers/pinctrl/pinctrl-sandbox.c
index 755ac08..0786afe 100644
--- a/drivers/pinctrl/pinctrl-sandbox.c
+++ b/drivers/pinctrl/pinctrl-sandbox.c
@@ -17,6 +17,14 @@ static const char * const sandbox_pins[] = {
 	"W1"
 };
 
+static const char * const sandbox_pins_muxing[] = {
+	"I2C SCL",
+	"I2C SDA",
+	"Uart TX",
+	"Uart RX",
+	"1-wire gpio",
+};
+
 static const char * const sandbox_groups[] = {
 	"i2c",
 	"serial_a",
@@ -56,6 +64,15 @@ static const char *sandbox_get_pin_name(struct udevice *dev, unsigned selector)
 	return sandbox_pins[selector];
 }
 
+static int sandbox_get_pin_muxing(struct udevice *dev,
+				  unsigned int selector,
+				  char *buf, int size)
+{
+	snprintf(buf, size, "%s", sandbox_pins_muxing[selector]);
+
+	return 0;
+}
+
 static int sandbox_get_groups_count(struct udevice *dev)
 {
 	return ARRAY_SIZE(sandbox_groups);
@@ -123,6 +140,7 @@ static int sandbox_pinconf_group_set(struct udevice *dev,
 const struct pinctrl_ops sandbox_pinctrl_ops = {
 	.get_pins_count = sandbox_get_pins_count,
 	.get_pin_name = sandbox_get_pin_name,
+	.get_pin_muxing = sandbox_get_pin_muxing,
 	.get_groups_count = sandbox_get_groups_count,
 	.get_group_name = sandbox_get_group_name,
 	.get_functions_count = sandbox_get_functions_count,
diff --git a/drivers/pinctrl/pinctrl-stmfx.c b/drivers/pinctrl/pinctrl-stmfx.c
new file mode 100644
index 0000000..9b5009a
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-stmfx.c
@@ -0,0 +1,414 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * Driver for STMicroelectronics Multi-Function eXpander (STMFX) GPIO expander
+ * based on Linux driver : pinctrl/pinctrl-stmfx.c
+ */
+#include <common.h>
+#include <dm.h>
+#include <i2c.h>
+#include <asm/gpio.h>
+#include <dm/device.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+#include <dm/pinctrl.h>
+#include <linux/bitfield.h>
+#include <power/regulator.h>
+
+/* STMFX pins = GPIO[15:0] + aGPIO[7:0] */
+#define STMFX_MAX_GPIO			16
+#define STMFX_MAX_AGPIO			8
+
+/* General */
+#define STMFX_REG_CHIP_ID		0x00 /* R */
+#define STMFX_REG_FW_VERSION_MSB	0x01 /* R */
+#define STMFX_REG_FW_VERSION_LSB	0x02 /* R */
+#define STMFX_REG_SYS_CTRL		0x40 /* RW */
+
+/* MFX boot time is around 10ms, so after reset, we have to wait this delay */
+#define STMFX_BOOT_TIME_MS 10
+
+/* GPIOs expander */
+/* GPIO_STATE1 0x10, GPIO_STATE2 0x11, GPIO_STATE3 0x12 */
+#define STMFX_REG_GPIO_STATE		0x10 /* R */
+/* GPIO_DIR1 0x60, GPIO_DIR2 0x61, GPIO_DIR3 0x63 */
+#define STMFX_REG_GPIO_DIR		0x60 /* RW */
+/* GPIO_TYPE1 0x64, GPIO_TYPE2 0x65, GPIO_TYPE3 0x66 */
+#define STMFX_REG_GPIO_TYPE		0x64 /* RW */
+/* GPIO_PUPD1 0x68, GPIO_PUPD2 0x69, GPIO_PUPD3 0x6A */
+#define STMFX_REG_GPIO_PUPD		0x68 /* RW */
+/* GPO_SET1 0x6C, GPO_SET2 0x6D, GPO_SET3 0x6E */
+#define STMFX_REG_GPO_SET		0x6C /* RW */
+/* GPO_CLR1 0x70, GPO_CLR2 0x71, GPO_CLR3 0x72 */
+#define STMFX_REG_GPO_CLR		0x70 /* RW */
+
+/* STMFX_REG_CHIP_ID bitfields */
+#define STMFX_REG_CHIP_ID_MASK		GENMASK(7, 0)
+
+/* STMFX_REG_SYS_CTRL bitfields */
+#define STMFX_REG_SYS_CTRL_GPIO_EN	BIT(0)
+#define STMFX_REG_SYS_CTRL_ALTGPIO_EN	BIT(3)
+#define STMFX_REG_SYS_CTRL_SWRST	BIT(7)
+
+#define NR_GPIO_REGS			3
+#define NR_GPIOS_PER_REG		8
+#define get_reg(offset)			((offset) / NR_GPIOS_PER_REG)
+#define get_shift(offset)		((offset) % NR_GPIOS_PER_REG)
+#define get_mask(offset)		(BIT(get_shift(offset)))
+
+struct stmfx_pinctrl {
+	struct udevice *gpio;
+};
+
+static int stmfx_read(struct udevice *dev, uint offset)
+{
+	return  dm_i2c_reg_read(dev_get_parent(dev), offset);
+}
+
+static int stmfx_write(struct udevice *dev, uint offset, unsigned int val)
+{
+	return dm_i2c_reg_write(dev_get_parent(dev), offset, val);
+}
+
+static int stmfx_gpio_get(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_STATE + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+
+	return ret < 0 ? ret : !!(ret & mask);
+}
+
+static int stmfx_gpio_set(struct udevice *dev, unsigned int offset, int value)
+{
+	u32 reg = value ? STMFX_REG_GPO_SET : STMFX_REG_GPO_CLR;
+	u32 mask = get_mask(offset);
+
+	return stmfx_write(dev, reg + get_reg(offset), mask);
+}
+
+static int stmfx_gpio_get_function(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+
+	if (ret < 0)
+		return ret;
+	/* On stmfx, gpio pins direction is (0)input, (1)output. */
+
+	return ret & mask ? GPIOF_OUTPUT : GPIOF_INPUT;
+}
+
+static int stmfx_gpio_direction_input(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+
+	ret &= ~mask;
+
+	return stmfx_write(dev, reg, ret & ~mask);
+}
+
+static int stmfx_gpio_direction_output(struct udevice *dev,
+				       unsigned int offset, int value)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_gpio_set(dev, offset, value);
+	if (ret < 0)
+		return ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+
+	return stmfx_write(dev, reg, ret | mask);
+}
+
+static int stmfx_gpio_probe(struct udevice *dev)
+{
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct ofnode_phandle_args args;
+	u8 sys_ctrl;
+
+	uc_priv->bank_name = "stmfx";
+	uc_priv->gpio_count = STMFX_MAX_GPIO + STMFX_MAX_AGPIO;
+	if (!dev_read_phandle_with_args(dev, "gpio-ranges",
+					NULL, 3, 0, &args)) {
+		uc_priv->gpio_count = args.args[2];
+	}
+
+	/* enable GPIO function */
+	sys_ctrl = STMFX_REG_SYS_CTRL_GPIO_EN;
+	if (uc_priv->gpio_count > STMFX_MAX_GPIO)
+		sys_ctrl |= STMFX_REG_SYS_CTRL_ALTGPIO_EN;
+	stmfx_write(dev, STMFX_REG_SYS_CTRL, sys_ctrl);
+
+	return 0;
+}
+
+static const struct dm_gpio_ops stmfx_gpio_ops = {
+	.set_value = stmfx_gpio_set,
+	.get_value = stmfx_gpio_get,
+	.get_function = stmfx_gpio_get_function,
+	.direction_input = stmfx_gpio_direction_input,
+	.direction_output = stmfx_gpio_direction_output,
+};
+
+U_BOOT_DRIVER(stmfx_gpio) = {
+	.name	= "stmfx-gpio",
+	.id	= UCLASS_GPIO,
+	.probe	= stmfx_gpio_probe,
+	.ops	= &stmfx_gpio_ops,
+};
+
+#if CONFIG_IS_ENABLED(PINCONF)
+static const struct pinconf_param stmfx_pinctrl_conf_params[] = {
+	{ "bias-disable", PIN_CONFIG_BIAS_DISABLE, 0 },
+	{ "bias-pull-up", PIN_CONFIG_BIAS_PULL_UP, 0 },
+	{ "bias-pull-pin-default", PIN_CONFIG_BIAS_PULL_PIN_DEFAULT, 0 },
+	{ "bias-pull-down", PIN_CONFIG_BIAS_PULL_DOWN, 0 },
+	{ "drive-open-drain", PIN_CONFIG_DRIVE_OPEN_DRAIN, 0 },
+	{ "drive-push-pull", PIN_CONFIG_DRIVE_PUSH_PULL, 0 },
+	{ "output-high", PIN_CONFIG_OUTPUT, 1 },
+	{ "output-low", PIN_CONFIG_OUTPUT, 0 },
+};
+
+static int stmfx_pinctrl_set_pupd(struct udevice *dev,
+				  unsigned int pin, u32 pupd)
+{
+	u8 reg = STMFX_REG_GPIO_PUPD + get_reg(pin);
+	u32 mask = get_mask(pin);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (pupd ? mask : 0);
+
+	return stmfx_write(dev, reg, ret);
+}
+
+static int stmfx_pinctrl_set_type(struct udevice *dev,
+				  unsigned int pin, u32 type)
+{
+	u8 reg = STMFX_REG_GPIO_TYPE + get_reg(pin);
+	u32 mask = get_mask(pin);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (type ? mask : 0);
+
+	return stmfx_write(dev, reg, ret);
+}
+
+static int stmfx_pinctrl_conf_set(struct udevice *dev, unsigned int pin,
+				  unsigned int param, unsigned int arg)
+{
+	int ret, dir;
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	dir = stmfx_gpio_get_function(plat->gpio, pin);
+
+	if (dir < 0)
+		return dir;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_PULL_PIN_DEFAULT:
+	case PIN_CONFIG_BIAS_DISABLE:
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		ret = stmfx_pinctrl_set_pupd(dev, pin, 0);
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		ret = stmfx_pinctrl_set_pupd(dev, pin, 1);
+		break;
+	case PIN_CONFIG_DRIVE_OPEN_DRAIN:
+		if (dir == GPIOF_OUTPUT)
+			ret = stmfx_pinctrl_set_type(dev, pin, 1);
+		else
+			ret = stmfx_pinctrl_set_type(dev, pin, 0);
+		break;
+	case PIN_CONFIG_DRIVE_PUSH_PULL:
+		if (dir == GPIOF_OUTPUT)
+			ret = stmfx_pinctrl_set_type(dev, pin, 0);
+		else
+			ret = stmfx_pinctrl_set_type(dev, pin, 1);
+		break;
+	case PIN_CONFIG_OUTPUT:
+		ret = stmfx_gpio_direction_output(plat->gpio, pin, arg);
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	return ret;
+}
+#endif
+
+static int stmfx_pinctrl_get_pins_count(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+	struct gpio_dev_priv *uc_priv;
+
+	uc_priv = dev_get_uclass_priv(plat->gpio);
+
+	return uc_priv->gpio_count;
+}
+
+/*
+ * STMFX pins[15:0] are called "gpio[15:0]"
+ * and STMFX pins[23:16] are called "agpio[7:0]"
+ */
+#define MAX_PIN_NAME_LEN 7
+static char pin_name[MAX_PIN_NAME_LEN];
+static const char *stmfx_pinctrl_get_pin_name(struct udevice *dev,
+					      unsigned int selector)
+{
+	if (selector < STMFX_MAX_GPIO)
+		snprintf(pin_name, MAX_PIN_NAME_LEN, "gpio%u", selector);
+	else
+		snprintf(pin_name, MAX_PIN_NAME_LEN, "agpio%u", selector - 16);
+	return pin_name;
+}
+
+static int stmfx_pinctrl_bind(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	return device_bind_driver_to_node(dev->parent,
+					  "stmfx-gpio", "stmfx-gpio",
+					  dev_ofnode(dev), &plat->gpio);
+};
+
+static int stmfx_pinctrl_probe(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	return device_probe(plat->gpio);
+};
+
+const struct pinctrl_ops stmfx_pinctrl_ops = {
+	.get_pins_count = stmfx_pinctrl_get_pins_count,
+	.get_pin_name = stmfx_pinctrl_get_pin_name,
+	.set_state = pinctrl_generic_set_state,
+#if CONFIG_IS_ENABLED(PINCONF)
+	.pinconf_set = stmfx_pinctrl_conf_set,
+	.pinconf_num_params = ARRAY_SIZE(stmfx_pinctrl_conf_params),
+	.pinconf_params = stmfx_pinctrl_conf_params,
+#endif
+};
+
+static const struct udevice_id stmfx_pinctrl_match[] = {
+	{ .compatible = "st,stmfx-0300-pinctrl", },
+};
+
+U_BOOT_DRIVER(stmfx_pinctrl) = {
+	.name = "stmfx-pinctrl",
+	.id = UCLASS_PINCTRL,
+	.of_match = of_match_ptr(stmfx_pinctrl_match),
+	.bind = stmfx_pinctrl_bind,
+	.probe = stmfx_pinctrl_probe,
+	.ops = &stmfx_pinctrl_ops,
+	.platdata_auto_alloc_size = sizeof(struct stmfx_pinctrl),
+};
+
+static int stmfx_chip_init(struct udevice *dev)
+{
+	u8 id;
+	u8 version[2];
+	int ret;
+	struct dm_i2c_chip *chip = dev_get_parent_platdata(dev);
+
+	id = dm_i2c_reg_read(dev, STMFX_REG_CHIP_ID);
+	if (id < 0) {
+		dev_err(dev, "error reading chip id: %d\n", id);
+		return ret;
+	}
+	/*
+	 * Check that ID is the complement of the I2C address:
+	 * STMFX I2C address follows the 7-bit format (MSB), that's why
+	 * client->addr is shifted.
+	 *
+	 * STMFX_I2C_ADDR|       STMFX         |        Linux
+	 *   input pin   | I2C device address  | I2C device address
+	 *---------------------------------------------------------
+	 *       0       | b: 1000 010x h:0x84 |       0x42
+	 *       1       | b: 1000 011x h:0x86 |       0x43
+	 */
+	if (FIELD_GET(STMFX_REG_CHIP_ID_MASK, ~id) != (chip->chip_addr << 1)) {
+		dev_err(dev, "unknown chip id: %#x\n", id);
+		return -EINVAL;
+	}
+
+	ret = dm_i2c_read(dev, STMFX_REG_FW_VERSION_MSB,
+			  version, sizeof(version));
+	if (ret) {
+		dev_err(dev, "error reading fw version: %d\n", ret);
+		return ret;
+	}
+
+	dev_info(dev, "STMFX id: %#x, fw version: %x.%02x\n",
+		 id, version[0], version[1]);
+
+	ret = dm_i2c_reg_read(dev, STMFX_REG_SYS_CTRL);
+
+	if (ret < 0)
+		return ret;
+
+	ret = dm_i2c_reg_write(dev, STMFX_REG_SYS_CTRL,
+			       ret | STMFX_REG_SYS_CTRL_SWRST);
+	if (ret)
+		return ret;
+
+	mdelay(STMFX_BOOT_TIME_MS);
+
+	return ret;
+}
+
+static int stmfx_probe(struct udevice *dev)
+{
+	struct udevice *vdd;
+	int ret;
+
+	ret = device_get_supply_regulator(dev, "vdd-supply", &vdd);
+	if (ret && ret != -ENOENT) {
+		dev_err(dev, "vdd regulator error:%d\n", ret);
+		return ret;
+	}
+	if (!ret) {
+		ret = regulator_set_enable(vdd, true);
+		if (ret) {
+			dev_err(dev, "vdd enable failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return stmfx_chip_init(dev);
+}
+
+static const struct udevice_id stmfx_match[] = {
+	{ .compatible = "st,stmfx-0300", },
+};
+
+U_BOOT_DRIVER(stmfx) = {
+	.name = "stmfx",
+	.id = UCLASS_I2C_GENERIC,
+	.of_match = of_match_ptr(stmfx_match),
+	.probe = stmfx_probe,
+	.bind = dm_scan_fdt_dev,
+};
diff --git a/drivers/pinctrl/pinctrl-uclass.c b/drivers/pinctrl/pinctrl-uclass.c
index c38bb21..3608f10 100644
--- a/drivers/pinctrl/pinctrl-uclass.c
+++ b/drivers/pinctrl/pinctrl-uclass.c
@@ -127,6 +127,9 @@ static int pinconfig_post_bind(struct udevice *dev)
 		ofnode_get_property(node, "compatible", &ret);
 		if (ret >= 0)
 			continue;
+		/* If this node has "gpio-controller" property, skip */
+		if (ofnode_read_bool(node, "gpio-controller"))
+			continue;
 
 		if (ret != -FDT_ERR_NOTFOUND)
 			return ret;
@@ -183,7 +186,7 @@ static int pinctrl_select_state_simple(struct udevice *dev)
 	 * is the correct one.  This is most likely OK as there is
 	 * usually only one pinctrl device on the system.
 	 */
-	ret = uclass_get_device(UCLASS_PINCTRL, 0, &pctldev);
+	ret = uclass_get_device_by_seq(UCLASS_PINCTRL, 0, &pctldev);
 	if (ret)
 		return ret;
 
@@ -249,6 +252,40 @@ int pinctrl_get_gpio_mux(struct udevice *dev, int banknum, int index)
 	return ops->get_gpio_mux(dev, banknum, index);
 }
 
+int pinctrl_get_pins_count(struct udevice *dev)
+{
+	struct pinctrl_ops *ops = pinctrl_get_ops(dev);
+
+	if (!ops->get_pins_count)
+		return -ENOSYS;
+
+	return ops->get_pins_count(dev);
+}
+
+int pinctrl_get_pin_name(struct udevice *dev, int selector, char *buf,
+			 int size)
+{
+	struct pinctrl_ops *ops = pinctrl_get_ops(dev);
+
+	if (!ops->get_pin_name)
+		return -ENOSYS;
+
+	snprintf(buf, size, ops->get_pin_name(dev, selector));
+
+	return 0;
+}
+
+int pinctrl_get_pin_muxing(struct udevice *dev, int selector, char *buf,
+			   int size)
+{
+	struct pinctrl_ops *ops = pinctrl_get_ops(dev);
+
+	if (!ops->get_pin_muxing)
+		return -ENOSYS;
+
+	return ops->get_pin_muxing(dev, selector, buf, size);
+}
+
 /**
  * pinconfig_post_bind() - post binding for PINCTRL uclass
  * Recursively bind child nodes as pinconfig devices in case of full pinctrl.
diff --git a/drivers/pinctrl/pinctrl_stm32.c b/drivers/pinctrl/pinctrl_stm32.c
index 31285cd..eb7799d 100644
--- a/drivers/pinctrl/pinctrl_stm32.c
+++ b/drivers/pinctrl/pinctrl_stm32.c
@@ -1,9 +1,10 @@
 #include <common.h>
 #include <dm.h>
-#include <dm/pinctrl.h>
+#include <hwspinlock.h>
 #include <asm/arch/gpio.h>
 #include <asm/gpio.h>
 #include <asm/io.h>
+#include <dm/pinctrl.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -14,17 +15,241 @@ DECLARE_GLOBAL_DATA_PTR;
 #define OTYPE_MSK			1
 #define AFR_MASK			0xF
 
+struct stm32_pinctrl_priv {
+	struct hwspinlock hws;
+	int pinctrl_ngpios;
+	struct list_head gpio_dev;
+};
+
+#ifndef CONFIG_SPL_BUILD
+struct stm32_gpio_bank {
+	struct udevice *gpio_dev;
+	struct list_head list;
+};
+
+static char pin_name[PINNAME_SIZE];
+#define PINMUX_MODE_COUNT		5
+static const char * const pinmux_mode[PINMUX_MODE_COUNT] = {
+	"gpio input",
+	"gpio output",
+	"analog",
+	"unknown",
+	"alt function",
+};
+
+static int stm32_pinctrl_get_af(struct udevice *dev, unsigned int offset)
+{
+	struct stm32_gpio_priv *priv = dev_get_priv(dev);
+	struct stm32_gpio_regs *regs = priv->regs;
+	u32 af;
+	u32 alt_shift = (offset % 8) * 4;
+	u32 alt_index =  offset / 8;
+
+	af = (readl(&regs->afr[alt_index]) &
+	      GENMASK(alt_shift + 3, alt_shift)) >> alt_shift;
+
+	return af;
+}
+
+static int stm32_populate_gpio_dev_list(struct udevice *dev)
+{
+	struct stm32_pinctrl_priv *priv = dev_get_priv(dev);
+	struct udevice *gpio_dev;
+	struct udevice *child;
+	struct stm32_gpio_bank *gpio_bank;
+	int ret;
+
+	/*
+	 * parse pin-controller sub-nodes (ie gpio bank nodes) and fill
+	 * a list with all gpio device reference which belongs to the
+	 * current pin-controller. This list is used to find pin_name and
+	 * pin muxing
+	 */
+	list_for_each_entry(child, &dev->child_head, sibling_node) {
+		ret = uclass_get_device_by_name(UCLASS_GPIO, child->name,
+						&gpio_dev);
+		if (ret < 0)
+			continue;
+
+		gpio_bank = malloc(sizeof(*gpio_bank));
+		if (!gpio_bank) {
+			dev_err(dev, "Not enough memory\n");
+			return -ENOMEM;
+		}
+
+		gpio_bank->gpio_dev = gpio_dev;
+		list_add_tail(&gpio_bank->list, &priv->gpio_dev);
+	}
+
+	return 0;
+}
+
+static int stm32_pinctrl_get_pins_count(struct udevice *dev)
+{
+	struct stm32_pinctrl_priv *priv = dev_get_priv(dev);
+	struct gpio_dev_priv *uc_priv;
+	struct stm32_gpio_bank *gpio_bank;
+
+	/*
+	 * if get_pins_count has already been executed once on this
+	 * pin-controller, no need to run it again
+	 */
+	if (priv->pinctrl_ngpios)
+		return priv->pinctrl_ngpios;
+
+	if (list_empty(&priv->gpio_dev))
+		stm32_populate_gpio_dev_list(dev);
+	/*
+	 * walk through all banks to retrieve the pin-controller
+	 * pins number
+	 */
+	list_for_each_entry(gpio_bank, &priv->gpio_dev, list) {
+		uc_priv = dev_get_uclass_priv(gpio_bank->gpio_dev);
+
+		priv->pinctrl_ngpios += uc_priv->gpio_count;
+	}
+
+	return priv->pinctrl_ngpios;
+}
+
+static struct udevice *stm32_pinctrl_get_gpio_dev(struct udevice *dev,
+						  unsigned int selector,
+						  unsigned int *idx)
+{
+	struct stm32_pinctrl_priv *priv = dev_get_priv(dev);
+	struct stm32_gpio_bank *gpio_bank;
+	struct gpio_dev_priv *uc_priv;
+	int pin_count = 0;
+
+	if (list_empty(&priv->gpio_dev))
+		stm32_populate_gpio_dev_list(dev);
+
+	/* look up for the bank which owns the requested pin */
+	list_for_each_entry(gpio_bank, &priv->gpio_dev, list) {
+		uc_priv = dev_get_uclass_priv(gpio_bank->gpio_dev);
+
+		if (selector < (pin_count + uc_priv->gpio_count)) {
+			/*
+			 * we found the bank, convert pin selector to
+			 * gpio bank index
+			 */
+			*idx = stm32_offset_to_index(gpio_bank->gpio_dev,
+						     selector - pin_count);
+			if (*idx < 0)
+				return NULL;
+
+			return gpio_bank->gpio_dev;
+		}
+		pin_count += uc_priv->gpio_count;
+	}
+
+	return NULL;
+}
+
+static const char *stm32_pinctrl_get_pin_name(struct udevice *dev,
+					      unsigned int selector)
+{
+	struct gpio_dev_priv *uc_priv;
+	struct udevice *gpio_dev;
+	unsigned int gpio_idx;
+
+	/* look up for the bank which owns the requested pin */
+	gpio_dev = stm32_pinctrl_get_gpio_dev(dev, selector, &gpio_idx);
+	if (!gpio_dev) {
+		snprintf(pin_name, PINNAME_SIZE, "Error");
+	} else {
+		uc_priv = dev_get_uclass_priv(gpio_dev);
+
+		snprintf(pin_name, PINNAME_SIZE, "%s%d",
+			 uc_priv->bank_name,
+			 gpio_idx);
+	}
+
+	return pin_name;
+}
+
+static int stm32_pinctrl_get_pin_muxing(struct udevice *dev,
+					unsigned int selector,
+					char *buf,
+					int size)
+{
+	struct udevice *gpio_dev;
+	const char *label;
+	int mode;
+	int af_num;
+	unsigned int gpio_idx;
+
+	/* look up for the bank which owns the requested pin */
+	gpio_dev = stm32_pinctrl_get_gpio_dev(dev, selector, &gpio_idx);
+
+	if (!gpio_dev)
+		return -ENODEV;
+
+	mode = gpio_get_raw_function(gpio_dev, gpio_idx, &label);
+
+	dev_dbg(dev, "selector = %d gpio_idx = %d mode = %d\n",
+		selector, gpio_idx, mode);
+
+
+	switch (mode) {
+	case GPIOF_UNKNOWN:
+		/* should never happen */
+		return -EINVAL;
+	case GPIOF_UNUSED:
+		snprintf(buf, size, "%s", pinmux_mode[mode]);
+		break;
+	case GPIOF_FUNC:
+		af_num = stm32_pinctrl_get_af(gpio_dev, gpio_idx);
+		snprintf(buf, size, "%s %d", pinmux_mode[mode], af_num);
+		break;
+	case GPIOF_OUTPUT:
+	case GPIOF_INPUT:
+		snprintf(buf, size, "%s %s",
+			 pinmux_mode[mode], label ? label : "");
+		break;
+	}
+
+	return 0;
+}
+#endif
+
+int stm32_pinctrl_probe(struct udevice *dev)
+{
+	struct stm32_pinctrl_priv *priv = dev_get_priv(dev);
+	int err;
+
+	/* hwspinlock property is optional, just log the error */
+	err = hwspinlock_get_by_index(dev, 0, &priv->hws);
+	if (err)
+		debug("%s: hwspinlock_get_by_index may have failed (%d)\n",
+		      __func__, err);
+
+	INIT_LIST_HEAD(&priv->gpio_dev);
+
+	return 0;
+}
+
 static int stm32_gpio_config(struct gpio_desc *desc,
 			     const struct stm32_gpio_ctl *ctl)
 {
 	struct stm32_gpio_priv *priv = dev_get_priv(desc->dev);
 	struct stm32_gpio_regs *regs = priv->regs;
+	struct stm32_pinctrl_priv *pinctrl_priv;
+	int ret;
 	u32 index;
 
 	if (!ctl || ctl->af > 15 || ctl->mode > 3 || ctl->otype > 1 ||
 	    ctl->pupd > 2 || ctl->speed > 3)
 		return -EINVAL;
 
+	pinctrl_priv = dev_get_priv(dev_get_parent(desc->dev));
+
+	ret = hwspinlock_lock_timeout(&pinctrl_priv->hws, 10);
+	if (ret == -ETIME) {
+		dev_err(desc->dev, "HWSpinlock timeout\n");
+		return ret;
+	}
+
 	index = (desc->offset & 0x07) * 4;
 	clrsetbits_le32(&regs->afr[desc->offset >> 3], AFR_MASK << index,
 			ctl->af << index);
@@ -39,6 +264,8 @@ static int stm32_gpio_config(struct gpio_desc *desc,
 	index = desc->offset;
 	clrsetbits_le32(&regs->otyper, OTYPE_MSK << index, ctl->otype << index);
 
+	hwspinlock_unlock(&pinctrl_priv->hws);
+
 	return 0;
 }
 
@@ -100,9 +327,9 @@ static int stm32_pinctrl_config(int offset)
 	int rv, len;
 
 	/*
-	 * check for "pinmux" property in each subnode (e.g. pins1 and pins2 for
-	 * usart1) of pin controller phandle "pinctrl-0"
-	 * */
+	 * check for "pinmux" property in each subnode of pin controller
+	 * phandle "pinctrl-0" (e.g. pins1 and pins2 for usart1)
+	 */
 	fdt_for_each_subnode(offset, gd->fdt_blob, offset) {
 		struct stm32_gpio_dsc gpio_dsc;
 		struct stm32_gpio_ctl gpio_ctl;
@@ -182,6 +409,11 @@ static struct pinctrl_ops stm32_pinctrl_ops = {
 #else /* PINCTRL_FULL */
 	.set_state_simple	= stm32_pinctrl_set_state_simple,
 #endif /* PINCTRL_FULL */
+#ifndef CONFIG_SPL_BUILD
+	.get_pin_name		= stm32_pinctrl_get_pin_name,
+	.get_pins_count		= stm32_pinctrl_get_pins_count,
+	.get_pin_muxing		= stm32_pinctrl_get_pin_muxing,
+#endif
 };
 
 static const struct udevice_id stm32_pinctrl_ids[] = {
@@ -195,9 +427,11 @@ static const struct udevice_id stm32_pinctrl_ids[] = {
 };
 
 U_BOOT_DRIVER(pinctrl_stm32) = {
-	.name		= "pinctrl_stm32",
-	.id		= UCLASS_PINCTRL,
-	.of_match	= stm32_pinctrl_ids,
-	.ops		= &stm32_pinctrl_ops,
-	.bind		= dm_scan_fdt_dev,
+	.name			= "pinctrl_stm32",
+	.id			= UCLASS_PINCTRL,
+	.of_match		= stm32_pinctrl_ids,
+	.ops			= &stm32_pinctrl_ops,
+	.bind			= dm_scan_fdt_dev,
+	.probe			= stm32_pinctrl_probe,
+	.priv_auto_alloc_size	= sizeof(struct stm32_pinctrl_priv),
 };
diff --git a/drivers/power/pmic/Kconfig b/drivers/power/pmic/Kconfig
index cba48e1..6b8f5b4 100644
--- a/drivers/power/pmic/Kconfig
+++ b/drivers/power/pmic/Kconfig
@@ -217,10 +217,10 @@ config DM_PMIC_TPS65910
 	DC-DC converter, 8 LDOs and a RTC. This driver binds the SMPS and LDO
 	pmic children.
 
-config PMIC_STPMU1
-	bool "Enable support for STMicroelectronics STPMU1 PMIC"
+config PMIC_STPMIC1
+	bool "Enable support for STMicroelectronics STPMIC1 PMIC"
 	depends on DM_PMIC && DM_I2C
 	---help---
-	The STPMU1 PMIC provides 4 BUCKs, 6 LDOs, 1 VREF and 2 power switches.
+	The STPMIC1 PMIC provides 4 BUCKs, 6 LDOs, 1 VREF and 2 power switches.
 	It is accessed via an I2C interface. The device is used with STM32MP1
 	SoCs. This driver implements register read/write operations.
diff --git a/drivers/power/pmic/Makefile b/drivers/power/pmic/Makefile
index 29ca442..4ba6bf6 100644
--- a/drivers/power/pmic/Makefile
+++ b/drivers/power/pmic/Makefile
@@ -22,7 +22,7 @@ obj-$(CONFIG_DM_PMIC_TPS65910) += pmic_tps65910_dm.o
 obj-$(CONFIG_$(SPL_)PMIC_PALMAS) += palmas.o
 obj-$(CONFIG_$(SPL_)PMIC_LP873X) += lp873x.o
 obj-$(CONFIG_$(SPL_)PMIC_LP87565) += lp87565.o
-obj-$(CONFIG_PMIC_STPMU1) += stpmu1.o
+obj-$(CONFIG_PMIC_STPMIC1) += stpmic1.o
 
 obj-$(CONFIG_POWER_LTC3676) += pmic_ltc3676.o
 obj-$(CONFIG_POWER_MAX77696) += pmic_max77696.o
diff --git a/drivers/power/pmic/stpmic1.c b/drivers/power/pmic/stpmic1.c
new file mode 100644
index 0000000..18f46fc
--- /dev/null
+++ b/drivers/power/pmic/stpmic1.c
@@ -0,0 +1,258 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <i2c.h>
+#include <sysreset.h>
+#include <dm/device.h>
+#include <dm/lists.h>
+#include <power/pmic.h>
+#include <power/stpmic1.h>
+
+#define STPMIC1_NUM_OF_REGS 0x100
+
+#if CONFIG_IS_ENABLED(DM_REGULATOR)
+#define STPMIC1_NVM_SIZE 8
+#define STPMIC1_NVM_POLL_TIMEOUT 100000
+#define STPMIC1_NVM_START_ADDRESS 0xf8
+
+enum pmic_nvm_op {
+	SHADOW_READ,
+	SHADOW_WRITE,
+	NVM_READ,
+	NVM_WRITE,
+};
+
+static const struct pmic_child_info stpmic1_children_info[] = {
+	{ .prefix = "ldo", .driver = "stpmic1_ldo" },
+	{ .prefix = "buck", .driver = "stpmic1_buck" },
+	{ .prefix = "vref_ddr", .driver = "stpmic1_vref_ddr" },
+	{ .prefix = "pwr_sw", .driver = "stpmic1_pwr_sw" },
+	{ .prefix = "boost", .driver = "stpmic1_boost" },
+	{ },
+};
+#endif /* DM_REGULATOR */
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int stpmic1_reg_count(struct udevice *dev)
+{
+	return STPMIC1_NUM_OF_REGS;
+}
+
+static int stpmic1_write(struct udevice *dev, uint reg, const uint8_t *buff,
+			 int len)
+{
+	int ret;
+
+	ret = dm_i2c_write(dev, reg, buff, len);
+	if (ret)
+		dev_err(dev, "%s: failed to write register %#x :%d",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int stpmic1_read(struct udevice *dev, uint reg, uint8_t *buff, int len)
+{
+	int ret;
+
+	ret = dm_i2c_read(dev, reg, buff, len);
+	if (ret)
+		dev_err(dev, "%s: failed to read register %#x : %d",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int stpmic1_bind(struct udevice *dev)
+{
+#if CONFIG_IS_ENABLED(DM_REGULATOR)
+	ofnode regulators_node;
+	int children;
+
+	regulators_node = dev_read_subnode(dev, "regulators");
+	if (!ofnode_valid(regulators_node)) {
+		dev_dbg(dev, "regulators subnode not found!");
+		return -ENXIO;
+	}
+	dev_dbg(dev, "found regulators subnode\n");
+
+	children = pmic_bind_children(dev, regulators_node,
+				      stpmic1_children_info);
+	if (!children)
+		dev_dbg(dev, "no child found\n");
+#endif /* DM_REGULATOR */
+
+	if (CONFIG_IS_ENABLED(SYSRESET))
+		return device_bind_driver(dev, "stpmic1-sysreset",
+					  "stpmic1-sysreset", NULL);
+
+	return 0;
+}
+
+static struct dm_pmic_ops stpmic1_ops = {
+	.reg_count = stpmic1_reg_count,
+	.read = stpmic1_read,
+	.write = stpmic1_write,
+};
+
+static const struct udevice_id stpmic1_ids[] = {
+	{ .compatible = "st,stpmic1" },
+	{ }
+};
+
+U_BOOT_DRIVER(pmic_stpmic1) = {
+	.name = "stpmic1_pmic",
+	.id = UCLASS_PMIC,
+	.of_match = stpmic1_ids,
+	.bind = stpmic1_bind,
+	.ops = &stpmic1_ops,
+};
+
+#ifndef CONFIG_SPL_BUILD
+static int stpmic1_nvm_rw(u8 addr, u8 *buf, int buf_len, enum pmic_nvm_op op)
+{
+	struct udevice *dev;
+	unsigned long timeout;
+	u8 cmd = STPMIC1_NVM_CMD_READ;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_PMIC,
+					  DM_GET_DRIVER(pmic_stpmic1), &dev);
+	if (ret)
+		/* No PMIC on power discrete board */
+		return -EOPNOTSUPP;
+
+	if (addr < STPMIC1_NVM_START_ADDRESS)
+		return -EACCES;
+
+	if (op == SHADOW_READ)
+		return pmic_read(dev, addr, buf, buf_len);
+
+	if (op == SHADOW_WRITE)
+		return pmic_write(dev, addr, buf, buf_len);
+
+	if (op == NVM_WRITE) {
+		cmd = STPMIC1_NVM_CMD_PROGRAM;
+
+		ret = pmic_write(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = pmic_reg_read(dev, STPMIC1_NVM_CR);
+	if (ret < 0)
+		return ret;
+
+	ret = pmic_reg_write(dev, STPMIC1_NVM_CR, ret | cmd);
+	if (ret < 0)
+		return ret;
+
+	timeout = timer_get_us() + STPMIC1_NVM_POLL_TIMEOUT;
+	for (;;) {
+		ret = pmic_reg_read(dev, STPMIC1_NVM_SR);
+		if (ret < 0)
+			return ret;
+
+		if (!(ret & STPMIC1_NVM_BUSY))
+			break;
+
+		if (time_after(timer_get_us(), timeout))
+			break;
+	}
+
+	if (ret & STPMIC1_NVM_BUSY)
+		return -ETIMEDOUT;
+
+	if (op == NVM_READ) {
+		ret = pmic_read(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+int stpmic1_shadow_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_READ);
+}
+
+int stpmic1_shadow_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_WRITE);
+}
+
+int stpmic1_nvm_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_READ);
+}
+
+int stpmic1_nvm_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_WRITE);
+}
+
+int stpmic1_nvm_read_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_READ);
+}
+
+int stpmic1_nvm_write_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_WRITE);
+}
+#endif /* CONFIG_SPL_BUILD */
+
+#ifdef CONFIG_SYSRESET
+static int stpmic1_sysreset_request(struct udevice *dev, enum sysreset_t type)
+{
+	struct udevice *pmic_dev;
+	int ret;
+
+	if (type != SYSRESET_POWER)
+		return -EPROTONOSUPPORT;
+
+	ret = uclass_get_device_by_driver(UCLASS_PMIC,
+					  DM_GET_DRIVER(pmic_stpmic1),
+					  &pmic_dev);
+
+	if (ret)
+		return -EOPNOTSUPP;
+
+	ret = pmic_reg_read(pmic_dev, STPMIC1_MAIN_CR);
+	if (ret < 0)
+		return ret;
+
+	ret = pmic_reg_write(pmic_dev, STPMIC1_MAIN_CR,
+			     ret | STPMIC1_SWOFF | STPMIC1_RREQ_EN);
+	if (ret < 0)
+		return ret;
+
+	return -EINPROGRESS;
+}
+
+static struct sysreset_ops stpmic1_sysreset_ops = {
+	.request = stpmic1_sysreset_request,
+};
+
+U_BOOT_DRIVER(stpmic1_sysreset) = {
+	.name = "stpmic1-sysreset",
+	.id = UCLASS_SYSRESET,
+	.ops = &stpmic1_sysreset_ops,
+};
+#endif
diff --git a/drivers/power/pmic/stpmu1.c b/drivers/power/pmic/stpmu1.c
deleted file mode 100644
index 82351b6..0000000
--- a/drivers/power/pmic/stpmu1.c
+++ /dev/null
@@ -1,95 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#include <common.h>
-#include <dm.h>
-#include <errno.h>
-#include <i2c.h>
-#include <power/pmic.h>
-#include <power/stpmu1.h>
-
-#define STMPU1_NUM_OF_REGS 0x100
-
-#ifndef CONFIG_SPL_BUILD
-static const struct pmic_child_info stpmu1_children_info[] = {
-	{ .prefix = "ldo", .driver = "stpmu1_ldo" },
-	{ .prefix = "buck", .driver = "stpmu1_buck" },
-	{ .prefix = "vref_ddr", .driver = "stpmu1_vref_ddr" },
-	{ .prefix = "pwr_sw", .driver = "stpmu1_pwr_sw" },
-	{ .prefix = "boost", .driver = "stpmu1_boost" },
-	{ },
-};
-#endif /* CONFIG_SPL_BUILD */
-
-static int stpmu1_reg_count(struct udevice *dev)
-{
-	return STMPU1_NUM_OF_REGS;
-}
-
-static int stpmu1_write(struct udevice *dev, uint reg, const uint8_t *buff,
-			int len)
-{
-	int ret;
-
-	ret = dm_i2c_write(dev, reg, buff, len);
-	if (ret)
-		dev_err(dev, "%s: failed to write register %#x :%d",
-			__func__, reg, ret);
-
-	return ret;
-}
-
-static int stpmu1_read(struct udevice *dev, uint reg, uint8_t *buff, int len)
-{
-	int ret;
-
-	ret = dm_i2c_read(dev, reg, buff, len);
-	if (ret)
-		dev_err(dev, "%s: failed to read register %#x : %d",
-			__func__, reg, ret);
-
-	return ret;
-}
-
-static int stpmu1_bind(struct udevice *dev)
-{
-#ifndef CONFIG_SPL_BUILD
-	ofnode regulators_node;
-	int children;
-
-	regulators_node = dev_read_subnode(dev, "regulators");
-	if (!ofnode_valid(regulators_node)) {
-		dev_dbg(dev, "regulators subnode not found!");
-		return -ENXIO;
-	}
-	dev_dbg(dev, "found regulators subnode\n");
-
-	children = pmic_bind_children(dev, regulators_node,
-				      stpmu1_children_info);
-	if (!children)
-		dev_dbg(dev, "no child found\n");
-#endif /* CONFIG_SPL_BUILD */
-
-	return 0;
-}
-
-static struct dm_pmic_ops stpmu1_ops = {
-	.reg_count = stpmu1_reg_count,
-	.read = stpmu1_read,
-	.write = stpmu1_write,
-};
-
-static const struct udevice_id stpmu1_ids[] = {
-	{ .compatible = "st,stpmu1" },
-	{ }
-};
-
-U_BOOT_DRIVER(pmic_stpmu1) = {
-	.name = "stpmu1_pmic",
-	.id = UCLASS_PMIC,
-	.of_match = stpmu1_ids,
-	.bind = stpmu1_bind,
-	.ops = &stpmu1_ops,
-};
diff --git a/drivers/power/regulator/Kconfig b/drivers/power/regulator/Kconfig
index 2561a8a..268f7ca 100644
--- a/drivers/power/regulator/Kconfig
+++ b/drivers/power/regulator/Kconfig
@@ -221,11 +221,17 @@ config DM_REGULATOR_TPS65910
 	regulator types of the TPS65910 (BUCK, BOOST and LDO). It implements
 	the get/set api for value and enable.
 
-config DM_REGULATOR_STPMU1
-	bool "Enable driver for STPMU1 regulators"
-	depends on DM_REGULATOR && PMIC_STPMU1
+config DM_REGULATOR_STPMIC1
+	bool "Enable driver for STPMIC1 regulators"
+	depends on DM_REGULATOR && PMIC_STPMIC1
 	---help---
-	Enable support for the regulator functions of the STPMU1 PMIC. The
+	Enable support for the regulator functions of the STPMIC1 PMIC. The
 	driver implements get/set api for the various BUCKS and LDOs supported
 	by the PMIC device. This driver is controlled by a device tree node
 	which includes voltage limits.
+
+config SPL_DM_REGULATOR_STPMIC1
+	bool "Enable driver for STPMIC1 regulators in SPL"
+	depends on SPL_DM_REGULATOR && PMIC_STPMIC1
+	---help---
+	Enable support for the regulator functions of the STPMIC1 PMIC in SPL.
diff --git a/drivers/power/regulator/Makefile b/drivers/power/regulator/Makefile
index a5f5683..7a8888a 100644
--- a/drivers/power/regulator/Makefile
+++ b/drivers/power/regulator/Makefile
@@ -23,4 +23,4 @@ obj-$(CONFIG_$(SPL_)DM_REGULATOR_LP873X) += lp873x_regulator.o
 obj-$(CONFIG_$(SPL_)DM_REGULATOR_LP87565) += lp87565_regulator.o
 obj-$(CONFIG_$(SPL_)DM_REGULATOR_STM32_VREFBUF) += stm32-vrefbuf.o
 obj-$(CONFIG_DM_REGULATOR_TPS65910) += tps65910_regulator.o
-obj-$(CONFIG_$(SPL_)DM_REGULATOR_STPMU1) += stpmu1.o
+obj-$(CONFIG_$(SPL_)DM_REGULATOR_STPMIC1) += stpmic1.o
diff --git a/drivers/power/regulator/fixed.c b/drivers/power/regulator/fixed.c
index a99aa78..872e394 100644
--- a/drivers/power/regulator/fixed.c
+++ b/drivers/power/regulator/fixed.c
@@ -35,7 +35,9 @@ static int fixed_regulator_ofdata_to_platdata(struct udevice *dev)
 	/* Set type to fixed */
 	uc_pdata->type = REGULATOR_TYPE_FIXED;
 
-	if (dev_read_bool(dev, "enable-active-high"))
+	if (!dev_read_bool(dev, "enable-active-high"))
+		flags |= GPIOD_ACTIVE_LOW;
+	if (uc_pdata->boot_on)
 		flags |= GPIOD_IS_OUT_ACTIVE;
 
 	/* Get fixed regulator optional enable GPIO desc */
diff --git a/drivers/power/regulator/regulator-uclass.c b/drivers/power/regulator/regulator-uclass.c
index 4da8e43..4511625 100644
--- a/drivers/power/regulator/regulator-uclass.c
+++ b/drivers/power/regulator/regulator-uclass.c
@@ -106,10 +106,15 @@ int regulator_get_enable(struct udevice *dev)
 int regulator_set_enable(struct udevice *dev, bool enable)
 {
 	const struct dm_regulator_ops *ops = dev_get_driver_ops(dev);
+	struct dm_regulator_uclass_platdata *uc_pdata;
 
 	if (!ops || !ops->set_enable)
 		return -ENOSYS;
 
+	uc_pdata = dev_get_uclass_platdata(dev);
+	if (!enable && uc_pdata->always_on)
+		return -EACCES;
+
 	return ops->set_enable(dev, enable);
 }
 
diff --git a/drivers/power/regulator/stpmic1.c b/drivers/power/regulator/stpmic1.c
new file mode 100644
index 0000000..50ef2a2
--- /dev/null
+++ b/drivers/power/regulator/stpmic1.c
@@ -0,0 +1,672 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ * Author: Christophe Kerello <christophe.kerello@st.com>
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <power/pmic.h>
+#include <power/regulator.h>
+#include <power/stpmic1.h>
+
+struct stpmic1_range {
+	int min_uv;
+	int min_sel;
+	int max_sel;
+	int step;
+};
+
+struct stpmic1_output {
+	const struct stpmic1_range *ranges;
+	int nbranges;
+};
+
+#define STPMIC1_MODE(_id, _val, _name) { \
+	.id = _id,			\
+	.register_value = _val,		\
+	.name = _name,			\
+}
+
+#define STPMIC1_RANGE(_min_uv, _min_sel, _max_sel, _step) { \
+	.min_uv = _min_uv,		\
+	.min_sel = _min_sel,		\
+	.max_sel = _max_sel,		\
+	.step = _step,			\
+}
+
+#define STPMIC1_OUTPUT(_ranges, _nbranges) { \
+	.ranges = _ranges,		\
+	.nbranges = _nbranges,		\
+}
+
+static int stpmic1_output_find_uv(int sel,
+				  const struct stpmic1_output *output)
+{
+	const struct stpmic1_range *range;
+	int i;
+
+	for (i = 0, range = output->ranges;
+	     i < output->nbranges; i++, range++) {
+		if (sel >= range->min_sel && sel <= range->max_sel)
+			return range->min_uv +
+			       (sel - range->min_sel) * range->step;
+	}
+
+	return -EINVAL;
+}
+
+static int stpmic1_output_find_sel(int uv,
+				   const struct stpmic1_output *output)
+{
+	const struct stpmic1_range *range;
+	int i;
+
+	for (i = 0, range = output->ranges;
+	     i < output->nbranges; i++, range++) {
+		if (uv == range->min_uv && !range->step)
+			return range->min_sel;
+
+		if (uv >= range->min_uv &&
+		    uv <= range->min_uv +
+			  (range->max_sel - range->min_sel) * range->step)
+			return range->min_sel +
+			       (uv - range->min_uv) / range->step;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * BUCK regulators
+ */
+
+static const struct stpmic1_range buck1_ranges[] = {
+	STPMIC1_RANGE(725000, 0, 4, 0),
+	STPMIC1_RANGE(725000, 5, 36, 25000),
+	STPMIC1_RANGE(1500000, 37, 63, 0),
+};
+
+static const struct stpmic1_range buck2_ranges[] = {
+	STPMIC1_RANGE(1000000, 0, 17, 0),
+	STPMIC1_RANGE(1050000, 18, 19, 0),
+	STPMIC1_RANGE(1100000, 20, 21, 0),
+	STPMIC1_RANGE(1150000, 22, 23, 0),
+	STPMIC1_RANGE(1200000, 24, 25, 0),
+	STPMIC1_RANGE(1250000, 26, 27, 0),
+	STPMIC1_RANGE(1300000, 28, 29, 0),
+	STPMIC1_RANGE(1350000, 30, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 33, 0),
+	STPMIC1_RANGE(1450000, 34, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 63, 0),
+};
+
+static const struct stpmic1_range buck3_ranges[] = {
+	STPMIC1_RANGE(1000000, 0, 19, 0),
+	STPMIC1_RANGE(1100000, 20, 23, 0),
+	STPMIC1_RANGE(1200000, 24, 27, 0),
+	STPMIC1_RANGE(1300000, 28, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 55, 100000),
+	STPMIC1_RANGE(3400000, 56, 63, 0),
+};
+
+static const struct stpmic1_range buck4_ranges[] = {
+	STPMIC1_RANGE(600000, 0, 27, 25000),
+	STPMIC1_RANGE(1300000, 28, 29, 0),
+	STPMIC1_RANGE(1350000, 30, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 33, 0),
+	STPMIC1_RANGE(1450000, 34, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 60, 100000),
+	STPMIC1_RANGE(3900000, 61, 63, 0),
+};
+
+/* BUCK: 1,2,3,4 - voltage ranges */
+static const struct stpmic1_output buck_voltage_range[] = {
+	STPMIC1_OUTPUT(buck1_ranges, ARRAY_SIZE(buck1_ranges)),
+	STPMIC1_OUTPUT(buck2_ranges, ARRAY_SIZE(buck2_ranges)),
+	STPMIC1_OUTPUT(buck3_ranges, ARRAY_SIZE(buck3_ranges)),
+	STPMIC1_OUTPUT(buck4_ranges, ARRAY_SIZE(buck4_ranges)),
+};
+
+/* BUCK modes */
+static const struct dm_regulator_mode buck_modes[] = {
+	STPMIC1_MODE(STPMIC1_PREG_MODE_HP, STPMIC1_PREG_MODE_HP, "HP"),
+	STPMIC1_MODE(STPMIC1_PREG_MODE_LP, STPMIC1_PREG_MODE_LP, "LP"),
+};
+
+static int stpmic1_buck_get_uv(struct udevice *dev, int buck)
+{
+	int sel;
+
+	sel = pmic_reg_read(dev, STPMIC1_BUCKX_MAIN_CR(buck));
+	if (sel < 0)
+		return sel;
+
+	sel &= STPMIC1_BUCK_VOUT_MASK;
+	sel >>= STPMIC1_BUCK_VOUT_SHIFT;
+
+	return stpmic1_output_find_uv(sel, &buck_voltage_range[buck]);
+}
+
+static int stpmic1_buck_get_value(struct udevice *dev)
+{
+	return stpmic1_buck_get_uv(dev->parent, dev->driver_data - 1);
+}
+
+static int stpmic1_buck_set_value(struct udevice *dev, int uv)
+{
+	int sel, buck = dev->driver_data - 1;
+
+	sel = stpmic1_output_find_sel(uv, &buck_voltage_range[buck]);
+	if (sel < 0)
+		return sel;
+
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_BUCKX_MAIN_CR(buck),
+			       STPMIC1_BUCK_VOUT_MASK,
+			       sel << STPMIC1_BUCK_VOUT_SHIFT);
+}
+
+static int stpmic1_buck_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_BUCK_ENA ? true : false;
+}
+
+static int stpmic1_buck_set_enable(struct udevice *dev, bool enable)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret, uv;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_buck_get_enable(dev) == enable)
+		return 0;
+
+	if (enable) {
+		uc_pdata = dev_get_uclass_platdata(dev);
+		uv = stpmic1_buck_get_value(dev);
+		if (uv < uc_pdata->min_uV || uv > uc_pdata->max_uV)
+			stpmic1_buck_set_value(dev, uc_pdata->min_uV);
+	}
+
+	ret = pmic_clrsetbits(dev->parent,
+			      STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1),
+			      STPMIC1_BUCK_ENA, enable ? STPMIC1_BUCK_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_buck_get_mode(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return ret;
+
+	return ret & STPMIC1_BUCK_PREG_MODE ? STPMIC1_PREG_MODE_LP :
+					      STPMIC1_PREG_MODE_HP;
+}
+
+static int stpmic1_buck_set_mode(struct udevice *dev, int mode)
+{
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1),
+			       STPMIC1_BUCK_PREG_MODE,
+			       mode ? STPMIC1_BUCK_PREG_MODE : 0);
+}
+
+static int stpmic1_buck_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_BUCK)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_BUCK;
+	uc_pdata->mode = (struct dm_regulator_mode *)buck_modes;
+	uc_pdata->mode_count = ARRAY_SIZE(buck_modes);
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_buck_ops = {
+	.get_value  = stpmic1_buck_get_value,
+	.set_value  = stpmic1_buck_set_value,
+	.get_enable = stpmic1_buck_get_enable,
+	.set_enable = stpmic1_buck_set_enable,
+	.get_mode   = stpmic1_buck_get_mode,
+	.set_mode   = stpmic1_buck_set_mode,
+};
+
+U_BOOT_DRIVER(stpmic1_buck) = {
+	.name = "stpmic1_buck",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_buck_ops,
+	.probe = stpmic1_buck_probe,
+};
+
+/*
+ * LDO regulators
+ */
+
+static const struct stpmic1_range ldo12_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 31, 0),
+};
+
+static const struct stpmic1_range ldo3_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 30, 0),
+	/* Sel 31 is special case when LDO3 is in mode sync_source (BUCK2/2) */
+};
+
+static const struct stpmic1_range ldo5_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 30, 100000),
+	STPMIC1_RANGE(3900000, 31, 31, 0),
+};
+
+static const struct stpmic1_range ldo6_ranges[] = {
+	STPMIC1_RANGE(900000, 0, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 31, 0),
+};
+
+/* LDO: 1,2,3,4,5,6 - voltage ranges */
+static const struct stpmic1_output ldo_voltage_range[] = {
+	STPMIC1_OUTPUT(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
+	STPMIC1_OUTPUT(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
+	STPMIC1_OUTPUT(ldo3_ranges, ARRAY_SIZE(ldo3_ranges)),
+	STPMIC1_OUTPUT(NULL, 0),
+	STPMIC1_OUTPUT(ldo5_ranges, ARRAY_SIZE(ldo5_ranges)),
+	STPMIC1_OUTPUT(ldo6_ranges, ARRAY_SIZE(ldo6_ranges)),
+};
+
+/* LDO modes */
+static const struct dm_regulator_mode ldo_modes[] = {
+	STPMIC1_MODE(STPMIC1_LDO_MODE_NORMAL,
+		     STPMIC1_LDO_MODE_NORMAL, "NORMAL"),
+	STPMIC1_MODE(STPMIC1_LDO_MODE_BYPASS,
+		     STPMIC1_LDO_MODE_BYPASS, "BYPASS"),
+	STPMIC1_MODE(STPMIC1_LDO_MODE_SINK_SOURCE,
+		     STPMIC1_LDO_MODE_SINK_SOURCE, "SINK SOURCE"),
+};
+
+static int stpmic1_ldo_get_value(struct udevice *dev)
+{
+	int sel, ldo = dev->driver_data - 1;
+
+	sel = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (sel < 0)
+		return sel;
+
+	/* ldo4 => 3,3V */
+	if (ldo == STPMIC1_LDO4)
+		return STPMIC1_LDO4_UV;
+
+	sel &= STPMIC1_LDO12356_VOUT_MASK;
+	sel >>= STPMIC1_LDO12356_VOUT_SHIFT;
+
+	/* ldo3, sel = 31 => BUCK2/2 */
+	if (ldo == STPMIC1_LDO3 && sel == STPMIC1_LDO3_DDR_SEL)
+		return stpmic1_buck_get_uv(dev->parent, STPMIC1_BUCK2) / 2;
+
+	return stpmic1_output_find_uv(sel, &ldo_voltage_range[ldo]);
+}
+
+static int stpmic1_ldo_set_value(struct udevice *dev, int uv)
+{
+	int sel, ldo = dev->driver_data - 1;
+
+	/* ldo4 => not possible */
+	if (ldo == STPMIC1_LDO4)
+		return -EINVAL;
+
+	sel = stpmic1_output_find_sel(uv, &ldo_voltage_range[ldo]);
+	if (sel < 0)
+		return sel;
+
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_LDOX_MAIN_CR(ldo),
+			       STPMIC1_LDO12356_VOUT_MASK,
+			       sel << STPMIC1_LDO12356_VOUT_SHIFT);
+}
+
+static int stpmic1_ldo_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_LDOX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_LDO_ENA ? true : false;
+}
+
+static int stpmic1_ldo_set_enable(struct udevice *dev, bool enable)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret, uv;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_ldo_get_enable(dev) == enable)
+		return 0;
+
+	if (enable) {
+		uc_pdata = dev_get_uclass_platdata(dev);
+		uv = stpmic1_ldo_get_value(dev);
+		if (uv < uc_pdata->min_uV || uv > uc_pdata->max_uV)
+			stpmic1_ldo_set_value(dev, uc_pdata->min_uV);
+	}
+
+	ret = pmic_clrsetbits(dev->parent,
+			      STPMIC1_LDOX_MAIN_CR(dev->driver_data - 1),
+			      STPMIC1_LDO_ENA, enable ? STPMIC1_LDO_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_ldo_get_mode(struct udevice *dev)
+{
+	int ret, ldo = dev->driver_data - 1;
+
+	if (ldo != STPMIC1_LDO3)
+		return -EINVAL;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (ret < 0)
+		return ret;
+
+	if (ret & STPMIC1_LDO3_MODE)
+		return STPMIC1_LDO_MODE_BYPASS;
+
+	ret &= STPMIC1_LDO12356_VOUT_MASK;
+	ret >>= STPMIC1_LDO12356_VOUT_SHIFT;
+
+	return ret == STPMIC1_LDO3_DDR_SEL ? STPMIC1_LDO_MODE_SINK_SOURCE :
+					     STPMIC1_LDO_MODE_NORMAL;
+}
+
+static int stpmic1_ldo_set_mode(struct udevice *dev, int mode)
+{
+	int ret, ldo = dev->driver_data - 1;
+
+	if (ldo != STPMIC1_LDO3)
+		return -EINVAL;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (ret < 0)
+		return ret;
+
+	switch (mode) {
+	case STPMIC1_LDO_MODE_SINK_SOURCE:
+		ret &= ~STPMIC1_LDO12356_VOUT_MASK;
+		ret |= STPMIC1_LDO3_DDR_SEL << STPMIC1_LDO12356_VOUT_SHIFT;
+	case STPMIC1_LDO_MODE_NORMAL:
+		ret &= ~STPMIC1_LDO3_MODE;
+		break;
+	case STPMIC1_LDO_MODE_BYPASS:
+		ret |= STPMIC1_LDO3_MODE;
+		break;
+	}
+
+	return pmic_reg_write(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo), ret);
+}
+
+static int stpmic1_ldo_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_LDO)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_LDO;
+	if (dev->driver_data - 1 == STPMIC1_LDO3) {
+		uc_pdata->mode = (struct dm_regulator_mode *)ldo_modes;
+		uc_pdata->mode_count = ARRAY_SIZE(ldo_modes);
+	} else {
+		uc_pdata->mode_count = 0;
+	}
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_ldo_ops = {
+	.get_value  = stpmic1_ldo_get_value,
+	.set_value  = stpmic1_ldo_set_value,
+	.get_enable = stpmic1_ldo_get_enable,
+	.set_enable = stpmic1_ldo_set_enable,
+	.get_mode   = stpmic1_ldo_get_mode,
+	.set_mode   = stpmic1_ldo_set_mode,
+};
+
+U_BOOT_DRIVER(stpmic1_ldo) = {
+	.name = "stpmic1_ldo",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_ldo_ops,
+	.probe = stpmic1_ldo_probe,
+};
+
+/*
+ * VREF DDR regulator
+ */
+
+static int stpmic1_vref_ddr_get_value(struct udevice *dev)
+{
+	/* BUCK2/2 */
+	return stpmic1_buck_get_uv(dev->parent, STPMIC1_BUCK2) / 2;
+}
+
+static int stpmic1_vref_ddr_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_REFDDR_MAIN_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_VREF_ENA ? true : false;
+}
+
+static int stpmic1_vref_ddr_set_enable(struct udevice *dev, bool enable)
+{
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_vref_ddr_get_enable(dev) == enable)
+		return 0;
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_REFDDR_MAIN_CR,
+			      STPMIC1_VREF_ENA, enable ? STPMIC1_VREF_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_vref_ddr_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_vref_ddr_ops = {
+	.get_value  = stpmic1_vref_ddr_get_value,
+	.get_enable = stpmic1_vref_ddr_get_enable,
+	.set_enable = stpmic1_vref_ddr_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_vref_ddr) = {
+	.name = "stpmic1_vref_ddr",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_vref_ddr_ops,
+	.probe = stpmic1_vref_ddr_probe,
+};
+
+/*
+ * BOOST regulator
+ */
+
+static int stpmic1_boost_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_BST_ON ? true : false;
+}
+
+static int stpmic1_boost_set_enable(struct udevice *dev, bool enable)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return ret;
+
+	if (!enable && ret & STPMIC1_PWR_SW_ON)
+		return -EINVAL;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (!!(ret & STPMIC1_BST_ON) == enable)
+		return 0;
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+			      STPMIC1_BST_ON,
+			      enable ? STPMIC1_BST_ON : 0);
+	if (enable)
+		mdelay(STPMIC1_USB_BOOST_START_UP_DELAY_MS);
+
+	return ret;
+}
+
+static int stpmic1_boost_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_boost_ops = {
+	.get_enable = stpmic1_boost_get_enable,
+	.set_enable = stpmic1_boost_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_boost) = {
+	.name = "stpmic1_boost",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_boost_ops,
+	.probe = stpmic1_boost_probe,
+};
+
+/*
+ * USB power switch
+ */
+
+static int stpmic1_pwr_sw_get_enable(struct udevice *dev)
+{
+	uint mask = 1 << dev->driver_data;
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & mask ? true : false;
+}
+
+static int stpmic1_pwr_sw_set_enable(struct udevice *dev, bool enable)
+{
+	uint mask = 1 << dev->driver_data;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return ret;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (!!(ret & mask) == enable)
+		return 0;
+
+	/* Boost management */
+	if (enable && !(ret & STPMIC1_BST_ON)) {
+		pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+				STPMIC1_BST_ON, STPMIC1_BST_ON);
+		mdelay(STPMIC1_USB_BOOST_START_UP_DELAY_MS);
+	} else if (!enable && ret & STPMIC1_BST_ON &&
+		   (ret & STPMIC1_PWR_SW_ON) != STPMIC1_PWR_SW_ON) {
+		pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+				STPMIC1_BST_ON, 0);
+	}
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+			      mask, enable ? mask : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_pwr_sw_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_PWR_SW)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_pwr_sw_ops = {
+	.get_enable = stpmic1_pwr_sw_get_enable,
+	.set_enable = stpmic1_pwr_sw_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_pwr_sw) = {
+	.name = "stpmic1_pwr_sw",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_pwr_sw_ops,
+	.probe = stpmic1_pwr_sw_probe,
+};
diff --git a/drivers/power/regulator/stpmu1.c b/drivers/power/regulator/stpmu1.c
deleted file mode 100644
index 6eb2420..0000000
--- a/drivers/power/regulator/stpmu1.c
+++ /dev/null
@@ -1,671 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- * Author: Christophe Kerello <christophe.kerello@st.com>
- */
-
-#include <common.h>
-#include <dm.h>
-#include <errno.h>
-#include <power/pmic.h>
-#include <power/regulator.h>
-#include <power/stpmu1.h>
-
-struct stpmu1_range {
-	int min_uv;
-	int min_sel;
-	int max_sel;
-	int step;
-};
-
-struct stpmu1_output_range {
-	const struct stpmu1_range *ranges;
-	int nbranges;
-};
-
-#define STPMU1_MODE(_id, _val, _name) { \
-	.id = _id,			\
-	.register_value = _val,		\
-	.name = _name,			\
-}
-
-#define STPMU1_RANGE(_min_uv, _min_sel, _max_sel, _step) { \
-	.min_uv = _min_uv,		\
-	.min_sel = _min_sel,		\
-	.max_sel = _max_sel,		\
-	.step = _step,			\
-}
-
-#define STPMU1_OUTPUT_RANGE(_ranges, _nbranges) { \
-	.ranges = _ranges,		\
-	.nbranges = _nbranges,		\
-}
-
-static int stpmu1_output_find_uv(int sel,
-				 const struct stpmu1_output_range *output_range)
-{
-	const struct stpmu1_range *range;
-	int i;
-
-	for (i = 0, range = output_range->ranges;
-	     i < output_range->nbranges; i++, range++) {
-		if (sel >= range->min_sel && sel <= range->max_sel)
-			return range->min_uv +
-			       (sel - range->min_sel) * range->step;
-	}
-
-	return -EINVAL;
-}
-
-static int stpmu1_output_find_sel(int uv,
-				  const struct stpmu1_output_range *output_range)
-{
-	const struct stpmu1_range *range;
-	int i;
-
-	for (i = 0, range = output_range->ranges;
-	     i < output_range->nbranges; i++, range++) {
-		if (uv == range->min_uv && !range->step)
-			return range->min_sel;
-
-		if (uv >= range->min_uv &&
-		    uv <= range->min_uv +
-			  (range->max_sel - range->min_sel) * range->step)
-			return range->min_sel +
-			       (uv - range->min_uv) / range->step;
-	}
-
-	return -EINVAL;
-}
-
-/*
- * BUCK regulators
- */
-
-static const struct stpmu1_range buck1_ranges[] = {
-	STPMU1_RANGE(600000, 0, 30, 25000),
-	STPMU1_RANGE(1350000, 31, 63, 0),
-};
-
-static const struct stpmu1_range buck2_ranges[] = {
-	STPMU1_RANGE(1000000, 0, 17, 0),
-	STPMU1_RANGE(1050000, 18, 19, 0),
-	STPMU1_RANGE(1100000, 20, 21, 0),
-	STPMU1_RANGE(1150000, 22, 23, 0),
-	STPMU1_RANGE(1200000, 24, 25, 0),
-	STPMU1_RANGE(1250000, 26, 27, 0),
-	STPMU1_RANGE(1300000, 28, 29, 0),
-	STPMU1_RANGE(1350000, 30, 31, 0),
-	STPMU1_RANGE(1400000, 32, 33, 0),
-	STPMU1_RANGE(1450000, 34, 35, 0),
-	STPMU1_RANGE(1500000, 36, 63, 0),
-};
-
-static const struct stpmu1_range buck3_ranges[] = {
-	STPMU1_RANGE(1000000, 0, 19, 0),
-	STPMU1_RANGE(1100000, 20, 23, 0),
-	STPMU1_RANGE(1200000, 24, 27, 0),
-	STPMU1_RANGE(1300000, 28, 31, 0),
-	STPMU1_RANGE(1400000, 32, 35, 0),
-	STPMU1_RANGE(1500000, 36, 55, 100000),
-	STPMU1_RANGE(3400000, 56, 63, 0),
-};
-
-static const struct stpmu1_range buck4_ranges[] = {
-	STPMU1_RANGE(600000, 0, 27, 25000),
-	STPMU1_RANGE(1300000, 28, 29, 0),
-	STPMU1_RANGE(1350000, 30, 31, 0),
-	STPMU1_RANGE(1400000, 32, 33, 0),
-	STPMU1_RANGE(1450000, 34, 35, 0),
-	STPMU1_RANGE(1500000, 36, 60, 100000),
-	STPMU1_RANGE(3900000, 61, 63, 0),
-};
-
-/* BUCK: 1,2,3,4 - voltage ranges */
-static const struct stpmu1_output_range buck_voltage_range[] = {
-	STPMU1_OUTPUT_RANGE(buck1_ranges, ARRAY_SIZE(buck1_ranges)),
-	STPMU1_OUTPUT_RANGE(buck2_ranges, ARRAY_SIZE(buck2_ranges)),
-	STPMU1_OUTPUT_RANGE(buck3_ranges, ARRAY_SIZE(buck3_ranges)),
-	STPMU1_OUTPUT_RANGE(buck4_ranges, ARRAY_SIZE(buck4_ranges)),
-};
-
-/* BUCK modes */
-static const struct dm_regulator_mode buck_modes[] = {
-	STPMU1_MODE(STPMU1_BUCK_MODE_HP, STPMU1_BUCK_MODE_HP, "HP"),
-	STPMU1_MODE(STPMU1_BUCK_MODE_LP, STPMU1_BUCK_MODE_LP, "LP"),
-};
-
-static int stpmu1_buck_get_uv(struct udevice *dev, int buck)
-{
-	int sel;
-
-	sel = pmic_reg_read(dev, STPMU1_BUCKX_CTRL_REG(buck));
-	if (sel < 0)
-		return sel;
-
-	sel &= STPMU1_BUCK_OUTPUT_MASK;
-	sel >>= STPMU1_BUCK_OUTPUT_SHIFT;
-
-	return stpmu1_output_find_uv(sel, &buck_voltage_range[buck]);
-}
-
-static int stpmu1_buck_get_value(struct udevice *dev)
-{
-	return stpmu1_buck_get_uv(dev->parent, dev->driver_data - 1);
-}
-
-static int stpmu1_buck_set_value(struct udevice *dev, int uv)
-{
-	int sel, buck = dev->driver_data - 1;
-
-	sel = stpmu1_output_find_sel(uv, &buck_voltage_range[buck]);
-	if (sel < 0)
-		return sel;
-
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_BUCKX_CTRL_REG(buck),
-			       STPMU1_BUCK_OUTPUT_MASK,
-			       sel << STPMU1_BUCK_OUTPUT_SHIFT);
-}
-
-static int stpmu1_buck_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_BUCK_EN ? true : false;
-}
-
-static int stpmu1_buck_set_enable(struct udevice *dev, bool enable)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret, uv;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_buck_get_enable(dev) == enable)
-		return 0;
-
-	if (enable) {
-		uc_pdata = dev_get_uclass_platdata(dev);
-		uv = stpmu1_buck_get_value(dev);
-		if ((uv < uc_pdata->min_uV) || (uv > uc_pdata->max_uV))
-			stpmu1_buck_set_value(dev, uc_pdata->min_uV);
-	}
-
-	ret = pmic_clrsetbits(dev->parent,
-			      STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1),
-			      STPMU1_BUCK_EN, enable ? STPMU1_BUCK_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_buck_get_mode(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return ret;
-
-	return ret & STPMU1_BUCK_MODE ? STPMU1_BUCK_MODE_LP :
-					 STPMU1_BUCK_MODE_HP;
-}
-
-static int stpmu1_buck_set_mode(struct udevice *dev, int mode)
-{
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1),
-			       STPMU1_BUCK_MODE,
-			       mode ? STPMU1_BUCK_MODE : 0);
-}
-
-static int stpmu1_buck_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_BUCK)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_BUCK;
-	uc_pdata->mode = (struct dm_regulator_mode *)buck_modes;
-	uc_pdata->mode_count = ARRAY_SIZE(buck_modes);
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_buck_ops = {
-	.get_value  = stpmu1_buck_get_value,
-	.set_value  = stpmu1_buck_set_value,
-	.get_enable = stpmu1_buck_get_enable,
-	.set_enable = stpmu1_buck_set_enable,
-	.get_mode   = stpmu1_buck_get_mode,
-	.set_mode   = stpmu1_buck_set_mode,
-};
-
-U_BOOT_DRIVER(stpmu1_buck) = {
-	.name = "stpmu1_buck",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_buck_ops,
-	.probe = stpmu1_buck_probe,
-};
-
-/*
- * LDO regulators
- */
-
-static const struct stpmu1_range ldo12_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 31, 0),
-};
-
-static const struct stpmu1_range ldo3_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 30, 0),
-	/* Sel 31 is special case when LDO3 is in mode sync_source (BUCK2/2) */
-};
-
-static const struct stpmu1_range ldo5_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 30, 100000),
-	STPMU1_RANGE(3900000, 31, 31, 0),
-};
-
-static const struct stpmu1_range ldo6_ranges[] = {
-	STPMU1_RANGE(900000, 0, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 31, 0),
-};
-
-/* LDO: 1,2,3,4,5,6 - voltage ranges */
-static const struct stpmu1_output_range ldo_voltage_range[] = {
-	STPMU1_OUTPUT_RANGE(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo3_ranges, ARRAY_SIZE(ldo3_ranges)),
-	STPMU1_OUTPUT_RANGE(NULL, 0),
-	STPMU1_OUTPUT_RANGE(ldo5_ranges, ARRAY_SIZE(ldo5_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo6_ranges, ARRAY_SIZE(ldo6_ranges)),
-};
-
-/* LDO modes */
-static const struct dm_regulator_mode ldo_modes[] = {
-	STPMU1_MODE(STPMU1_LDO_MODE_NORMAL,
-		    STPMU1_LDO_MODE_NORMAL, "NORMAL"),
-	STPMU1_MODE(STPMU1_LDO_MODE_BYPASS,
-		    STPMU1_LDO_MODE_BYPASS, "BYPASS"),
-	STPMU1_MODE(STPMU1_LDO_MODE_SINK_SOURCE,
-		    STPMU1_LDO_MODE_SINK_SOURCE, "SINK SOURCE"),
-};
-
-static int stpmu1_ldo_get_value(struct udevice *dev)
-{
-	int sel, ldo = dev->driver_data - 1;
-
-	sel = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (sel < 0)
-		return sel;
-
-	/* ldo4 => 3,3V */
-	if (ldo == STPMU1_LDO4)
-		return STPMU1_LDO4_UV;
-
-	sel &= STPMU1_LDO12356_OUTPUT_MASK;
-	sel >>= STPMU1_LDO12356_OUTPUT_SHIFT;
-
-	/* ldo3, sel = 31 => BUCK2/2 */
-	if (ldo == STPMU1_LDO3 && sel == STPMU1_LDO3_DDR_SEL)
-		return stpmu1_buck_get_uv(dev->parent, STPMU1_BUCK2) / 2;
-
-	return stpmu1_output_find_uv(sel, &ldo_voltage_range[ldo]);
-}
-
-static int stpmu1_ldo_set_value(struct udevice *dev, int uv)
-{
-	int sel, ldo = dev->driver_data - 1;
-
-	/* ldo4 => not possible */
-	if (ldo == STPMU1_LDO4)
-		return -EINVAL;
-
-	sel = stpmu1_output_find_sel(uv, &ldo_voltage_range[ldo]);
-	if (sel < 0)
-		return sel;
-
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_LDOX_CTRL_REG(ldo),
-			       STPMU1_LDO12356_OUTPUT_MASK,
-			       sel << STPMU1_LDO12356_OUTPUT_SHIFT);
-}
-
-static int stpmu1_ldo_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_LDOX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_LDO_EN ? true : false;
-}
-
-static int stpmu1_ldo_set_enable(struct udevice *dev, bool enable)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret, uv;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_ldo_get_enable(dev) == enable)
-		return 0;
-
-	if (enable) {
-		uc_pdata = dev_get_uclass_platdata(dev);
-		uv = stpmu1_ldo_get_value(dev);
-		if ((uv < uc_pdata->min_uV) || (uv > uc_pdata->max_uV))
-			stpmu1_ldo_set_value(dev, uc_pdata->min_uV);
-	}
-
-	ret = pmic_clrsetbits(dev->parent,
-			      STPMU1_LDOX_CTRL_REG(dev->driver_data - 1),
-			      STPMU1_LDO_EN, enable ? STPMU1_LDO_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_ldo_get_mode(struct udevice *dev)
-{
-	int ret, ldo = dev->driver_data - 1;
-
-	if (ldo != STPMU1_LDO3)
-		return -EINVAL;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (ret < 0)
-		return ret;
-
-	if (ret & STPMU1_LDO3_MODE)
-		return STPMU1_LDO_MODE_BYPASS;
-
-	ret &= STPMU1_LDO12356_OUTPUT_MASK;
-	ret >>= STPMU1_LDO12356_OUTPUT_SHIFT;
-
-	return ret == STPMU1_LDO3_DDR_SEL ? STPMU1_LDO_MODE_SINK_SOURCE :
-					     STPMU1_LDO_MODE_NORMAL;
-}
-
-static int stpmu1_ldo_set_mode(struct udevice *dev, int mode)
-{
-	int ret, ldo = dev->driver_data - 1;
-
-	if (ldo != STPMU1_LDO3)
-		return -EINVAL;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (ret < 0)
-		return ret;
-
-	switch (mode) {
-	case STPMU1_LDO_MODE_SINK_SOURCE:
-		ret &= ~STPMU1_LDO12356_OUTPUT_MASK;
-		ret |= STPMU1_LDO3_DDR_SEL << STPMU1_LDO12356_OUTPUT_SHIFT;
-	case STPMU1_LDO_MODE_NORMAL:
-		ret &= ~STPMU1_LDO3_MODE;
-		break;
-	case STPMU1_LDO_MODE_BYPASS:
-		ret |= STPMU1_LDO3_MODE;
-		break;
-	}
-
-	return pmic_reg_write(dev->parent, STPMU1_LDOX_CTRL_REG(ldo), ret);
-}
-
-static int stpmu1_ldo_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_LDO)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_LDO;
-	if (dev->driver_data - 1 == STPMU1_LDO3) {
-		uc_pdata->mode = (struct dm_regulator_mode *)ldo_modes;
-		uc_pdata->mode_count = ARRAY_SIZE(ldo_modes);
-	} else {
-		uc_pdata->mode_count = 0;
-	}
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_ldo_ops = {
-	.get_value  = stpmu1_ldo_get_value,
-	.set_value  = stpmu1_ldo_set_value,
-	.get_enable = stpmu1_ldo_get_enable,
-	.set_enable = stpmu1_ldo_set_enable,
-	.get_mode   = stpmu1_ldo_get_mode,
-	.set_mode   = stpmu1_ldo_set_mode,
-};
-
-U_BOOT_DRIVER(stpmu1_ldo) = {
-	.name = "stpmu1_ldo",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_ldo_ops,
-	.probe = stpmu1_ldo_probe,
-};
-
-/*
- * VREF DDR regulator
- */
-
-static int stpmu1_vref_ddr_get_value(struct udevice *dev)
-{
-	/* BUCK2/2 */
-	return stpmu1_buck_get_uv(dev->parent, STPMU1_BUCK2) / 2;
-}
-
-static int stpmu1_vref_ddr_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_VREF_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_VREF_EN ? true : false;
-}
-
-static int stpmu1_vref_ddr_set_enable(struct udevice *dev, bool enable)
-{
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_vref_ddr_get_enable(dev) == enable)
-		return 0;
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_VREF_CTRL_REG,
-			      STPMU1_VREF_EN, enable ? STPMU1_VREF_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_vref_ddr_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_vref_ddr_ops = {
-	.get_value  = stpmu1_vref_ddr_get_value,
-	.get_enable = stpmu1_vref_ddr_get_enable,
-	.set_enable = stpmu1_vref_ddr_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_vref_ddr) = {
-	.name = "stpmu1_vref_ddr",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_vref_ddr_ops,
-	.probe = stpmu1_vref_ddr_probe,
-};
-
-/*
- * BOOST regulator
- */
-
-static int stpmu1_boost_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_USB_BOOST_EN ? true : false;
-}
-
-static int stpmu1_boost_set_enable(struct udevice *dev, bool enable)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return ret;
-
-	if (!enable && ret & STPMU1_USB_PWR_SW_EN)
-		return -EINVAL;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (!!(ret & STPMU1_USB_BOOST_EN) == enable)
-		return 0;
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-			      STPMU1_USB_BOOST_EN,
-			      enable ? STPMU1_USB_BOOST_EN : 0);
-	if (enable)
-		mdelay(STPMU1_USB_BOOST_START_UP_DELAY_MS);
-
-	return ret;
-}
-
-static int stpmu1_boost_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_boost_ops = {
-	.get_enable = stpmu1_boost_get_enable,
-	.set_enable = stpmu1_boost_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_boost) = {
-	.name = "stpmu1_boost",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_boost_ops,
-	.probe = stpmu1_boost_probe,
-};
-
-/*
- * USB power switch
- */
-
-static int stpmu1_pwr_sw_get_enable(struct udevice *dev)
-{
-	uint mask = 1 << dev->driver_data;
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & mask ? true : false;
-}
-
-static int stpmu1_pwr_sw_set_enable(struct udevice *dev, bool enable)
-{
-	uint mask = 1 << dev->driver_data;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return ret;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (!!(ret & mask) == enable)
-		return 0;
-
-	/* Boost management */
-	if (enable && !(ret & STPMU1_USB_BOOST_EN)) {
-		pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-				STPMU1_USB_BOOST_EN, STPMU1_USB_BOOST_EN);
-		mdelay(STPMU1_USB_BOOST_START_UP_DELAY_MS);
-	} else if (!enable && ret & STPMU1_USB_BOOST_EN &&
-		   (ret & STPMU1_USB_PWR_SW_EN) != STPMU1_USB_PWR_SW_EN) {
-		pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-				STPMU1_USB_BOOST_EN, 0);
-	}
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-			      mask, enable ? mask : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_pwr_sw_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_PWR_SW)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_pwr_sw_ops = {
-	.get_enable = stpmu1_pwr_sw_get_enable,
-	.set_enable = stpmu1_pwr_sw_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_pwr_sw) = {
-	.name = "stpmu1_pwr_sw",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_pwr_sw_ops,
-	.probe = stpmu1_pwr_sw_probe,
-};
diff --git a/drivers/ram/stm32mp1/Kconfig b/drivers/ram/stm32mp1/Kconfig
index b9c8166..761de09 100644
--- a/drivers/ram/stm32mp1/Kconfig
+++ b/drivers/ram/stm32mp1/Kconfig
@@ -10,3 +10,32 @@ config STM32MP1_DDR
 		family:	support for LPDDR2, LPDDR3 and DDR3
 		the SDRAM parameters for controleur and phy need to be provided
 		in device tree (computed by DDR tuning tools)
+
+config STM32MP1_DDR_INTERACTIVE
+	bool "STM32MP1 DDR driver : interactive support"
+	depends on STM32MP1_DDR
+	help
+		activate interactive support in STM32MP1 DDR controller driver
+		used for DDR tuning tools
+		to enter in intercative mode type 'd' during SPL DDR driver
+		initialisation
+
+config STM32MP1_DDR_INTERACTIVE_FORCE
+	bool "STM32MP1 DDR driver : force interactive mode"
+	depends on STM32MP1_DDR_INTERACTIVE
+	default n
+	help
+		force interactive mode in STM32MP1 DDR controller driver
+		skip the polling of character 'd' in console
+		useful when SPL is loaded in sysram
+		directly by programmer
+
+config STM32MP1_DDR_TUNING
+	bool "STM32MP1 DDR driver : support of tuning"
+	depends on SPL && STM32MP1_DDR_INTERACTIVE
+	default y
+	help
+		activate tuning command in STM32MP1 DDR interactive mode
+		used for DDR tuning tools
+		- DQ Deskew algorithm
+		- DQS Trimming
diff --git a/drivers/ram/stm32mp1/Makefile b/drivers/ram/stm32mp1/Makefile
index 79eb028..bc292d1 100644
--- a/drivers/ram/stm32mp1/Makefile
+++ b/drivers/ram/stm32mp1/Makefile
@@ -5,3 +5,10 @@
 
 obj-y += stm32mp1_ram.o
 obj-y += stm32mp1_ddr.o
+
+obj-$(CONFIG_STM32MP1_DDR_INTERACTIVE) += stm32mp1_interactive.o stm32mp1_tests.o
+obj-$(CONFIG_STM32MP1_DDR_TUNING) += stm32mp1_tuning.o
+
+ifneq ($(DDR_INTERACTIVE),)
+CFLAGS_stm32mp1_interactive.o += -DCONFIG_STM32MP1_DDR_INTERACTIVE_FORCE=y
+endif
diff --git a/drivers/ram/stm32mp1/stm32mp1_ddr.c b/drivers/ram/stm32mp1/stm32mp1_ddr.c
index c7c3ba7..a83ea39 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ddr.c
+++ b/drivers/ram/stm32mp1/stm32mp1_ddr.c
@@ -165,6 +165,32 @@ static const struct reg_desc ddrphy_cal[] = {
 	DDRPHY_REG_CAL(dx3dqstr),
 };
 
+#define DDR_REG_DYN(x) \
+	{#x,\
+	 offsetof(struct stm32mp1_ddrctl, x),\
+	 INVALID_OFFSET}
+
+static const struct reg_desc ddr_dyn[] = {
+	DDR_REG_DYN(stat),
+	DDR_REG_DYN(init0),
+	DDR_REG_DYN(dfimisc),
+	DDR_REG_DYN(dfistat),
+	DDR_REG_DYN(swctl),
+	DDR_REG_DYN(swstat),
+	DDR_REG_DYN(pctrl_0),
+	DDR_REG_DYN(pctrl_1),
+};
+
+#define DDRPHY_REG_DYN(x) \
+	{#x,\
+	 offsetof(struct stm32mp1_ddrphy, x),\
+	 INVALID_OFFSET}
+
+static const struct reg_desc ddrphy_dyn[] = {
+	DDRPHY_REG_DYN(pir),
+	DDRPHY_REG_DYN(pgsr),
+};
+
 enum reg_type {
 	REG_REG,
 	REG_TIMING,
@@ -173,6 +199,11 @@ enum reg_type {
 	REGPHY_REG,
 	REGPHY_TIMING,
 	REGPHY_CAL,
+/* dynamic registers => managed in driver or not changed,
+ * can be dumped in interactive mode
+ */
+	REG_DYN,
+	REGPHY_DYN,
 	REG_TYPE_NB
 };
 
@@ -206,6 +237,10 @@ const struct ddr_reg_info ddr_registers[REG_TYPE_NB] = {
 	"timing", ddrphy_timing, ARRAY_SIZE(ddrphy_timing), DDRPHY_BASE},
 [REGPHY_CAL] = {
 	"cal", ddrphy_cal, ARRAY_SIZE(ddrphy_cal), DDRPHY_BASE},
+[REG_DYN] = {
+	"dyn", ddr_dyn, ARRAY_SIZE(ddr_dyn), DDR_BASE},
+[REGPHY_DYN] = {
+	"dyn", ddrphy_dyn, ARRAY_SIZE(ddrphy_dyn), DDRPHY_BASE},
 };
 
 const char *base_name[] = {
@@ -277,10 +312,247 @@ void stm32mp1_ddrphy_init(struct stm32mp1_ddrphy *phy, u32 pir)
 	ddrphy_idone_wait(phy);
 }
 
+#ifdef CONFIG_STM32MP1_DDR_INTERACTIVE
+static void stm32mp1_dump_reg_desc(u32 base_addr, const struct reg_desc *desc)
+{
+	unsigned int *ptr;
+
+	ptr = (unsigned int *)(base_addr + desc->offset);
+#ifdef DEBUG
+	printf("[0x%08x] %s= 0x%08x\n",
+	       (u32)ptr, desc->name, readl(ptr));
+#else
+	printf("%s= 0x%08x\n", desc->name, readl(ptr));
+#endif
+}
+
+static void stm32mp1_dump_param_desc(u32 par_addr, const struct reg_desc *desc)
+{
+	unsigned int *ptr;
+
+	ptr = (unsigned int *)(par_addr + desc->par_offset);
+	printf("%s= 0x%08x\n", desc->name, readl(ptr));
+}
+
+static const struct reg_desc *found_reg(const char *name, enum reg_type *type)
+{
+	unsigned int i, j;
+	const struct reg_desc *desc;
+
+	for (i = 0; i < ARRAY_SIZE(ddr_registers); i++) {
+		desc = ddr_registers[i].desc;
+		for (j = 0; j < ddr_registers[i].size; j++) {
+			if (strcmp(name, desc[j].name) == 0) {
+				*type = i;
+				return &desc[j];
+			}
+		}
+	}
+	*type = REG_TYPE_NB;
+	return NULL;
+}
+
+int stm32mp1_dump_reg(const struct ddr_info *priv,
+		      const char *name)
+{
+	unsigned int i, j;
+	const struct reg_desc *desc;
+	u32 base_addr;
+	enum base_type p_base;
+	enum reg_type type;
+	const char *p_name;
+	enum base_type filter = NONE_BASE;
+	int result = -1;
+
+	if (name) {
+		if (strcmp(name, base_name[DDR_BASE]) == 0)
+			filter = DDR_BASE;
+		else if (strcmp(name, base_name[DDRPHY_BASE]) == 0)
+			filter = DDRPHY_BASE;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ddr_registers); i++) {
+		p_base = ddr_registers[i].base;
+		p_name = ddr_registers[i].name;
+		if (!name || (filter == p_base || !strcmp(name, p_name))) {
+			result = 0;
+			desc = ddr_registers[i].desc;
+			base_addr = get_base_addr(priv, p_base);
+			printf("==%s.%s==\n", base_name[p_base], p_name);
+			for (j = 0; j < ddr_registers[i].size; j++)
+				stm32mp1_dump_reg_desc(base_addr, &desc[j]);
+		}
+	}
+	if (result) {
+		desc = found_reg(name, &type);
+		if (desc) {
+			p_base = ddr_registers[type].base;
+			base_addr = get_base_addr(priv, p_base);
+			stm32mp1_dump_reg_desc(base_addr, desc);
+			result = 0;
+		}
+	}
+	return result;
+}
+
+void stm32mp1_edit_reg(const struct ddr_info *priv,
+		       char *name, char *string)
+{
+	unsigned long *ptr, value;
+	enum reg_type type;
+	enum base_type base;
+	const struct reg_desc *desc;
+	u32 base_addr;
+
+	desc = found_reg(name, &type);
+
+	if (!desc) {
+		printf("%s not found\n", name);
+		return;
+	}
+	if (strict_strtoul(string, 16, &value) <  0) {
+		printf("invalid value %s\n", string);
+		return;
+	}
+	base = ddr_registers[type].base;
+	base_addr = get_base_addr(priv, base);
+	ptr = (unsigned long *)(base_addr + desc->offset);
+	writel(value, ptr);
+#ifdef DEBUG
+	printf("[0x%08x] %s= 0x%08lx (0x%08x)\n",
+	       (u32)ptr, desc->name, value, readl(ptr));
+#else
+	printf("%s= 0x%08x\n", desc->name, readl(ptr));
+#endif
+}
+
+static u32 get_par_addr(const struct stm32mp1_ddr_config *config,
+			enum reg_type type)
+{
+	u32 par_addr = 0x0;
+
+	switch (type) {
+	case REG_REG:
+		par_addr = (u32)&config->c_reg;
+		break;
+	case REG_TIMING:
+		par_addr = (u32)&config->c_timing;
+		break;
+	case REG_PERF:
+		par_addr = (u32)&config->c_perf;
+		break;
+	case REG_MAP:
+		par_addr = (u32)&config->c_map;
+		break;
+	case REGPHY_REG:
+		par_addr = (u32)&config->p_reg;
+		break;
+	case REGPHY_TIMING:
+		par_addr = (u32)&config->p_timing;
+		break;
+	case REGPHY_CAL:
+		par_addr = (u32)&config->p_cal;
+		break;
+	case REG_DYN:
+	case REGPHY_DYN:
+	case REG_TYPE_NB:
+		par_addr = (u32)NULL;
+		break;
+	}
+
+	return par_addr;
+}
+
+int stm32mp1_dump_param(const struct stm32mp1_ddr_config *config,
+			const char *name)
+{
+	unsigned int i, j;
+	const struct reg_desc *desc;
+	u32 par_addr;
+	enum base_type p_base;
+	enum reg_type type;
+	const char *p_name;
+	enum base_type filter = NONE_BASE;
+	int result = -EINVAL;
+
+	if (name) {
+		if (strcmp(name, base_name[DDR_BASE]) == 0)
+			filter = DDR_BASE;
+		else if (strcmp(name, base_name[DDRPHY_BASE]) == 0)
+			filter = DDRPHY_BASE;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ddr_registers); i++) {
+		par_addr = get_par_addr(config, i);
+		if (!par_addr)
+			continue;
+		p_base = ddr_registers[i].base;
+		p_name = ddr_registers[i].name;
+		if (!name || (filter == p_base || !strcmp(name, p_name))) {
+			result = 0;
+			desc = ddr_registers[i].desc;
+			printf("==%s.%s==\n", base_name[p_base], p_name);
+			for (j = 0; j < ddr_registers[i].size; j++)
+				stm32mp1_dump_param_desc(par_addr, &desc[j]);
+		}
+	}
+	if (result) {
+		desc = found_reg(name, &type);
+		if (desc) {
+			par_addr = get_par_addr(config, type);
+			if (par_addr) {
+				stm32mp1_dump_param_desc(par_addr, desc);
+				result = 0;
+			}
+		}
+	}
+	return result;
+}
+
+void stm32mp1_edit_param(const struct stm32mp1_ddr_config *config,
+			 char *name, char *string)
+{
+	unsigned long *ptr, value;
+	enum reg_type type;
+	const struct reg_desc *desc;
+	u32 par_addr;
+
+	desc = found_reg(name, &type);
+	if (!desc) {
+		printf("%s not found\n", name);
+		return;
+	}
+	if (strict_strtoul(string, 16, &value) < 0) {
+		printf("invalid value %s\n", string);
+		return;
+	}
+	par_addr = get_par_addr(config, type);
+	if (!par_addr) {
+		printf("no parameter %s\n", name);
+		return;
+	}
+	ptr = (unsigned long *)(par_addr + desc->par_offset);
+	writel(value, ptr);
+	printf("%s= 0x%08x\n", desc->name, readl(ptr));
+}
+#endif
+
+__weak bool stm32mp1_ddr_interactive(void *priv,
+				     enum stm32mp1_ddr_interact_step step,
+				     const struct stm32mp1_ddr_config *config)
+{
+	return false;
+}
+
+#define INTERACTIVE(step)\
+	stm32mp1_ddr_interactive(priv, step, config)
+
 /* start quasi dynamic register update */
 static void start_sw_done(struct stm32mp1_ddrctl *ctl)
 {
 	clrbits_le32(&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+	debug("[0x%08x] swctl = 0x%08x\n",
+	      (u32)&ctl->swctl,  readl(&ctl->swctl));
 }
 
 /* wait quasi dynamic register update */
@@ -312,7 +584,7 @@ static void wait_operating_mode(struct ddr_info *priv, int mode)
 	/* self-refresh due to software => check also STAT.selfref_type */
 	if (mode == DDRCTRL_STAT_OPERATING_MODE_SR) {
 		mask |= DDRCTRL_STAT_SELFREF_TYPE_MASK;
-		stat |= DDRCTRL_STAT_SELFREF_TYPE_SR;
+		val |= DDRCTRL_STAT_SELFREF_TYPE_SR;
 	} else if (mode == DDRCTRL_STAT_OPERATING_MODE_NORMAL) {
 		/* normal mode: handle also automatic self refresh */
 		mask2 = DDRCTRL_STAT_OPERATING_MODE_MASK |
@@ -332,6 +604,182 @@ static void wait_operating_mode(struct ddr_info *priv, int mode)
 	debug("[0x%08x] stat = 0x%08x\n", (u32)&priv->ctl->stat, stat);
 }
 
+/* Mode Register Writes (MRW or MRS) */
+void mode_register_write(struct ddr_info *priv, u8 addr, u16 data)
+{
+	u32 mrctrl0;
+
+	debug("MRS: %d = %x\n", addr, data);
+
+	/* 1. Poll MRSTAT.mr_wr_busy until it is '0'.
+	 *    This checks that there is no outstanding MR transaction.
+	 *    No writes should be performed to MRCTRL0 and MRCTRL1
+	 *    if MRSTAT.mr_wr_busy = 1.
+	 */
+	while (readl(&priv->ctl->mrstat) & DDRCTRL_MRSTAT_MR_WR_BUSY)
+		;
+
+	/* 2. Write the MRCTRL0.mr_type, MRCTRL0.mr_addr, MRCTRL0.mr_rank
+	 *    and (for MRWs) MRCTRL1.mr_data to define the MR transaction.
+	 */
+	mrctrl0 = DDRCTRL_MRCTRL0_MR_TYPE_WRITE |
+		  DDRCTRL_MRCTRL0_MR_RANK_ALL |
+		  ((addr << DDRCTRL_MRCTRL0_MR_ADDR_SHIFT)
+		   & DDRCTRL_MRCTRL0_MR_ADDR_MASK);
+	writel(mrctrl0, &priv->ctl->mrctrl0);
+	debug("[0x%08x] mrctrl0 = 0x%08x (0x%08x)\n",
+	      (u32)&priv->ctl->mrctrl0,
+	      readl(&priv->ctl->mrctrl0), mrctrl0);
+	writel(data, &priv->ctl->mrctrl1);
+	debug("[0x%08x] mrctrl1 = 0x%08x\n",
+	      (u32)&priv->ctl->mrctrl1, readl(&priv->ctl->mrctrl1));
+
+	/* 3. In a separate APB transaction, write the MRCTRL0.mr_wr to 1. This
+	 *    bit is self-clearing, and triggers the MR transaction.
+	 *    The uMCTL2 then asserts the MRSTAT.mr_wr_busy while it performs
+	 *    the MR transaction to SDRAM, and no further accesses can be
+	 *    initiated until it is deasserted
+	 */
+	mrctrl0 |= DDRCTRL_MRCTRL0_MR_WR;
+	writel(mrctrl0, &priv->ctl->mrctrl0);
+
+	while (readl(&priv->ctl->mrstat) & DDRCTRL_MRSTAT_MR_WR_BUSY)
+		;
+	debug("[0x%08x] mrctrl0 = 0x%08x\n",
+	      (u32)&priv->ctl->mrctrl0, mrctrl0);
+}
+
+/* switch DDR3 from DLL-on to DLL-off */
+static void ddr3_dll_off(struct ddr_info *priv)
+{
+	u32 mr1 = readl(&priv->phy->mr1);
+	u32 mr2 = readl(&priv->phy->mr2);
+	u32 dbgcam;
+
+	debug("%s: entry\n", __func__);
+	debug("mr1: 0x%08x\n", mr1);
+	debug("mr2: 0x%08x\n", mr2);
+	/* 1. Set the DBG1.dis_hif = 1.
+	 *    This prevents further reads/writes being received on the HIF.
+	 */
+	setbits_le32(&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	debug("[0x%08x] dbg1 = 0x%08x\n",
+	      (u32)&priv->ctl->dbg1, readl(&priv->ctl->dbg1));
+
+	/* 2. Ensure all commands have been flushed from the uMCTL2 by polling
+	 *    DBGCAM.wr_data_pipeline_empty = 1,
+	 *    DBGCAM.rd_data_pipeline_empty = 1,
+	 *    DBGCAM.dbg_wr_q_depth = 0 ,
+	 *    DBGCAM.dbg_lpr_q_depth = 0, and
+	 *    DBGCAM.dbg_hpr_q_depth = 0.
+	 */
+	do {
+		dbgcam = readl(&priv->ctl->dbgcam);
+		debug("[0x%08x] dbgcam = 0x%08x\n",
+		      (u32)&priv->ctl->dbgcam, dbgcam);
+	} while (((dbgcam & DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY) ==
+		   DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY) &&
+		 !(dbgcam & DDRCTRL_DBGCAM_DBG_Q_DEPTH));
+
+	/* 3. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to disable RTT_NOM:
+	 *    a. DDR3: Write to MR1[9], MR1[6] and MR1[2]
+	 *    b. DDR4: Write to MR1[10:8]
+	 */
+	mr1 &= ~(BIT(9) | BIT(6) | BIT(2));
+	mode_register_write(priv, 1, mr1);
+
+	/* 4. For DDR4 only: Perform an MRS command
+	 *    (using MRCTRL0 and MRCTRL1 registers) to write to MR5[8:6]
+	 *    to disable RTT_PARK
+	 */
+
+	/* 5. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to write to MR2[10:9], to disable RTT_WR
+	 *    (and therefore disable dynamic ODT).
+	 *    This applies for both DDR3 and DDR4.
+	 */
+	mr2 &= ~GENMASK(10, 9);
+	mode_register_write(priv, 2, mr2);
+
+	/* 6. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+	 *    to disable the DLL. The timing of this MRS is automatically
+	 *    handled by the uMCTL2.
+	 *    a. DDR3: Write to MR1[0]
+	 *    b. DDR4: Write to MR1[0]
+	 */
+	mr1 |= BIT(0);
+	mode_register_write(priv, 1, mr1);
+
+	/* 7. Put the SDRAM into self-refresh mode by setting
+	 *    PWRCTL.selfref_sw = 1, and polling STAT.operating_mode to ensure
+	 *    the DDRC has entered self-refresh.
+	 */
+	setbits_le32(&priv->ctl->pwrctl,
+		     DDRCTRL_PWRCTL_SELFREF_SW);
+	debug("[0x%08x] pwrctl = 0x%08x\n",
+	      (u32)&priv->ctl->pwrctl,
+	      readl(&priv->ctl->pwrctl));
+
+	/* 8. Wait until STAT.operating_mode[1:0]==11 indicating that the
+	 *    DWC_ddr_umctl2 core is in self-refresh mode.
+	 *    Ensure transition to self-refresh was due to software
+	 *    by checking that STAT.selfref_type[1:0]=2.
+	 */
+	wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_SR);
+
+	/* 9. Set the MSTR.dll_off_mode = 1.
+	 *    warning: MSTR.dll_off_mode is a quasi-dynamic type 2 field
+	 */
+	start_sw_done(priv->ctl);
+
+	setbits_le32(&priv->ctl->mstr, DDRCTRL_MSTR_DLL_OFF_MODE);
+	debug("[0x%08x] mstr = 0x%08x\n",
+	      (u32)&priv->ctl->mstr,
+	      readl(&priv->ctl->mstr));
+
+	wait_sw_done_ack(priv->ctl);
+
+	/* 10. Change the clock frequency to the desired value. */
+
+	/* 11. Update any registers which may be required to change for the new
+	 *     frequency. This includes static and dynamic registers.
+	 *     This includes both uMCTL2 registers and PHY registers.
+	 */
+	/* change Bypass Mode Frequency Range */
+	if (clk_get_rate(&priv->clk) < 100000000)
+		clrbits_le32(&priv->phy->dllgcr, DDRPHYC_DLLGCR_BPS200);
+	else
+		setbits_le32(&priv->phy->dllgcr, DDRPHYC_DLLGCR_BPS200);
+
+	setbits_le32(&priv->phy->acdllcr, DDRPHYC_ACDLLCR_DLLDIS);
+
+	setbits_le32(&priv->phy->dx0dllcr, DDRPHYC_DXNDLLCR_DLLDIS);
+	setbits_le32(&priv->phy->dx1dllcr, DDRPHYC_DXNDLLCR_DLLDIS);
+	setbits_le32(&priv->phy->dx2dllcr, DDRPHYC_DXNDLLCR_DLLDIS);
+	setbits_le32(&priv->phy->dx3dllcr, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* 12. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
+	clrbits_le32(&priv->ctl->pwrctl, DDRCTRL_PWRCTL_SELFREF_SW);
+	wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+
+	/* 13. If ZQCTL0.dis_srx_zqcl = 0, the uMCTL2 performs a ZQCL command
+	 *     at this point.
+	 */
+
+	/* 14. Perform MRS commands as required to re-program timing registers
+	 *     in the SDRAM for the new frequency
+	 *     (in particular, CL, CWL and WR may need to be changed).
+	 */
+
+	/* 15. Write DBG1.dis_hif = 0 to re-enable reads and writes.*/
+	clrbits_le32(&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	debug("[0x%08x] dbg1 = 0x%08x\n",
+	      (u32)&priv->ctl->dbg1, readl(&priv->ctl->dbg1));
+
+	debug("%s: exit\n", __func__);
+}
+
 void stm32mp1_refresh_disable(struct stm32mp1_ddrctl *ctl)
 {
 	start_sw_done(ctl);
@@ -355,7 +803,7 @@ void stm32mp1_refresh_restore(struct stm32mp1_ddrctl *ctl,
 }
 
 /* board-specific DDR power initializations. */
-__weak int board_ddr_power_init(void)
+__weak int board_ddr_power_init(enum ddr_type ddr_type)
 {
 	return 0;
 }
@@ -365,16 +813,25 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 		       const struct stm32mp1_ddr_config *config)
 {
 	u32 pir;
-	int ret;
+	int ret = -EINVAL;
 
-	ret = board_ddr_power_init();
+	if (config->c_reg.mstr & DDRCTRL_MSTR_DDR3)
+		ret = board_ddr_power_init(STM32MP_DDR3);
+	else if (config->c_reg.mstr & DDRCTRL_MSTR_LPDDR2)
+		ret = board_ddr_power_init(STM32MP_LPDDR2);
+	else if (config->c_reg.mstr & DDRCTRL_MSTR_LPDDR3)
+		ret = board_ddr_power_init(STM32MP_LPDDR3);
 
 	if (ret)
 		panic("ddr power init failed\n");
 
+start:
+	debug("%s entry\n", __func__);
+
 	debug("name = %s\n", config->info.name);
-	debug("speed = %d MHz\n", config->info.speed);
+	debug("speed = %d kHz\n", config->info.speed);
 	debug("size  = 0x%x\n", config->info.size);
+
 /*
  * 1. Program the DWC_ddr_umctl2 registers
  * 1.1 RESETS: presetn, core_ddrc_rstn, aresetn
@@ -389,7 +846,7 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 
 /* 1.2. start CLOCK */
 	if (stm32mp1_ddr_clk_enable(priv, config->info.speed))
-		panic("invalid DRAM clock : %d MHz\n",
+		panic("invalid DRAM clock : %d kHz\n",
 		      config->info.speed);
 
 /* 1.3. deassert reset */
@@ -401,11 +858,12 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	 */
 	clrbits_le32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAPBRST);
 
-/* 1.4. wait 4 cycles for synchronization */
-	asm(" nop");
-	asm(" nop");
-	asm(" nop");
-	asm(" nop");
+/* 1.4. wait 128 cycles to permit initialization of end logic */
+	udelay(2);
+	/* for PCLK = 133MHz => 1 us is enough, 2 to allow lower frequency */
+
+	if (INTERACTIVE(STEP_DDR_RESET))
+		goto start;
 
 /* 1.5. initialize registers ddr_umctl2 */
 	/* Stop uMCTL2 before PHY is ready */
@@ -414,6 +872,17 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	      (u32)&priv->ctl->dfimisc, readl(&priv->ctl->dfimisc));
 
 	set_reg(priv, REG_REG, &config->c_reg);
+
+	/* DDR3 = don't set DLLOFF for init mode */
+	if ((config->c_reg.mstr &
+	     (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+	    == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+		debug("deactivate DLL OFF in mstr\n");
+		clrbits_le32(&priv->ctl->mstr, DDRCTRL_MSTR_DLL_OFF_MODE);
+		debug("[0x%08x] mstr = 0x%08x\n",
+		      (u32)&priv->ctl->mstr,  readl(&priv->ctl->mstr));
+	}
+
 	set_reg(priv, REG_TIMING, &config->c_timing);
 	set_reg(priv, REG_MAP, &config->c_map);
 
@@ -421,9 +890,14 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	clrsetbits_le32(&priv->ctl->init0,
 			DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK,
 			DDRCTRL_INIT0_SKIP_DRAM_INIT_NORMAL);
+	debug("[0x%08x] init0 = 0x%08x\n",
+	      (u32)&priv->ctl->init0, readl(&priv->ctl->init0));
 
 	set_reg(priv, REG_PERF, &config->c_perf);
 
+	if (INTERACTIVE(STEP_CTL_INIT))
+		goto start;
+
 /*  2. deassert reset signal core_ddrc_rstn, aresetn and presetn */
 	clrbits_le32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCORERST);
 	clrbits_le32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAXIRST);
@@ -436,6 +910,19 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	set_reg(priv, REGPHY_TIMING, &config->p_timing);
 	set_reg(priv, REGPHY_CAL, &config->p_cal);
 
+	/* DDR3 = don't set DLLOFF for init mode */
+	if ((config->c_reg.mstr &
+	     (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+	    == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+		debug("deactivate DLL OFF in mr1\n");
+		clrbits_le32(&priv->phy->mr1, BIT(0));
+		debug("[0x%08x] mr1 = 0x%08x\n",
+		      (u32)&priv->phy->mr1,  readl(&priv->phy->mr1));
+	}
+
+	if (INTERACTIVE(STEP_PHY_INIT))
+		goto start;
+
 /*  4. Monitor PHY init status by polling PUBL register PGSR.IDONE
  *     Perform DDR PHY DRAM initialization and Gate Training Evaluation
  */
@@ -445,6 +932,7 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
  *     by setting PIR.INIT and PIR CTLDINIT and pool PGSR.IDONE
  *     DRAM init is done by PHY, init0.skip_dram.init = 1
  */
+
 	pir = DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK | DDRPHYC_PIR_ZCAL |
 	      DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_DRAMINIT | DDRPHYC_PIR_ICPC;
 
@@ -456,7 +944,12 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 /*  6. SET DFIMISC.dfi_init_complete_en to 1 */
 	/* Enable quasi-dynamic register programming*/
 	start_sw_done(priv->ctl);
+
 	setbits_le32(&priv->ctl->dfimisc, DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	debug("[0x%08x] dfimisc = 0x%08x\n",
+	      (u32)&priv->ctl->dfimisc,
+	      readl(&priv->ctl->dfimisc));
+
 	wait_sw_done_ack(priv->ctl);
 
 /*  7. Wait for DWC_ddr_umctl2 to move to normal operation mode
@@ -466,6 +959,10 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 
 	wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
 
+	/* switch to DLL OFF mode */
+	if (config->c_reg.mstr & DDRCTRL_MSTR_DLL_OFF_MODE)
+		ddr3_dll_off(priv);
+
 	debug("DDR DQS training : ");
 /*  8. Disable Auto refresh and power down by setting
  *    - RFSHCTL3.dis_au_refresh = 1
@@ -492,4 +989,7 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	/* enable uMCTL2 AXI port 0 and 1 */
 	setbits_le32(&priv->ctl->pctrl_0, DDRCTRL_PCTRL_N_PORT_EN);
 	setbits_le32(&priv->ctl->pctrl_1, DDRCTRL_PCTRL_N_PORT_EN);
+
+	if (INTERACTIVE(STEP_DDR_READY))
+		goto start;
 }
diff --git a/drivers/ram/stm32mp1/stm32mp1_ddr.h b/drivers/ram/stm32mp1/stm32mp1_ddr.h
index 3cd0161..90b4a4b 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ddr.h
+++ b/drivers/ram/stm32mp1/stm32mp1_ddr.h
@@ -26,6 +26,7 @@ struct stm32mp1_ddrphy;
  * @ctl: DDR controleur base address
  * @clk: DDR clock
  * @phy: DDR PHY base address
+ * @pwr: pwr base address
  * @rcc: rcc base address
  */
 struct ddr_info {
@@ -34,6 +35,7 @@ struct ddr_info {
 	struct clk clk;
 	struct stm32mp1_ddrctl *ctl;
 	struct stm32mp1_ddrphy *phy;
+	void *pwr;
 	u32 rcc;
 };
 
@@ -157,7 +159,7 @@ struct stm32mp1_ddrphy_cal {
 
 struct stm32mp1_ddr_info {
 	const char *name;
-	u16 speed; /* in MHZ */
+	u32 speed; /* in kHZ */
 	u32 size;  /* memory size in byte = col * row * width */
 };
 
@@ -172,7 +174,7 @@ struct stm32mp1_ddr_config {
 	struct stm32mp1_ddrphy_cal p_cal;
 };
 
-int stm32mp1_ddr_clk_enable(struct ddr_info *priv, u16 mem_speed);
+int stm32mp1_ddr_clk_enable(struct ddr_info *priv, u32 mem_speed);
 void stm32mp1_ddrphy_init(struct stm32mp1_ddrphy *phy, u32 pir);
 void stm32mp1_refresh_disable(struct stm32mp1_ddrctl *ctl);
 void stm32mp1_refresh_restore(struct stm32mp1_ddrctl *ctl,
@@ -197,10 +199,6 @@ void stm32mp1_edit_param(const struct stm32mp1_ddr_config *config,
 			 char *name,
 			 char *string);
 
-void stm32mp1_dump_info(
-	const struct ddr_info *priv,
-	const struct stm32mp1_ddr_config *config);
-
 bool stm32mp1_ddr_interactive(
 	void *priv,
 	enum stm32mp1_ddr_interact_step step,
diff --git a/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h b/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
index a606b2b..9d33186 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
+++ b/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
@@ -234,6 +234,8 @@ struct stm32mp1_ddrphy {
 
 /* DDRCTRL REGISTERS */
 #define DDRCTRL_MSTR_DDR3			BIT(0)
+#define DDRCTRL_MSTR_LPDDR2			BIT(2)
+#define DDRCTRL_MSTR_LPDDR3			BIT(3)
 #define DDRCTRL_MSTR_DATA_BUS_WIDTH_MASK	GENMASK(13, 12)
 #define DDRCTRL_MSTR_DATA_BUS_WIDTH_FULL	(0 << 12)
 #define DDRCTRL_MSTR_DATA_BUS_WIDTH_HALF	(1 << 12)
@@ -330,6 +332,7 @@ struct stm32mp1_ddrphy {
 
 #define DDRPHYC_DXNGCR_DXEN			BIT(0)
 
+#define DDRPHYC_DXNDLLCR_DLLSRST		BIT(30)
 #define DDRPHYC_DXNDLLCR_DLLDIS			BIT(31)
 #define DDRPHYC_DXNDLLCR_SDPHASE_MASK		GENMASK(17, 14)
 #define DDRPHYC_DXNDLLCR_SDPHASE_SHIFT		14
diff --git a/drivers/ram/stm32mp1/stm32mp1_interactive.c b/drivers/ram/stm32mp1/stm32mp1_interactive.c
new file mode 100644
index 0000000..3840af5
--- /dev/null
+++ b/drivers/ram/stm32mp1/stm32mp1_interactive.c
@@ -0,0 +1,467 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <console.h>
+#include <cli.h>
+#include <clk.h>
+#include <malloc.h>
+#include <ram.h>
+#include <reset.h>
+#include "stm32mp1_ddr.h"
+#include "stm32mp1_tests.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+enum ddr_command {
+	DDR_CMD_HELP,
+	DDR_CMD_INFO,
+	DDR_CMD_FREQ,
+	DDR_CMD_RESET,
+	DDR_CMD_PARAM,
+	DDR_CMD_PRINT,
+	DDR_CMD_EDIT,
+	DDR_CMD_STEP,
+	DDR_CMD_NEXT,
+	DDR_CMD_GO,
+	DDR_CMD_TEST,
+	DDR_CMD_TUNING,
+	DDR_CMD_UNKNOWN,
+};
+
+const char *step_str[] = {
+	[STEP_DDR_RESET] = "DDR_RESET",
+	[STEP_CTL_INIT] = "DDR_CTRL_INIT_DONE",
+	[STEP_PHY_INIT] = "DDR PHY_INIT_DONE",
+	[STEP_DDR_READY] = "DDR_READY",
+	[STEP_RUN] = "RUN"
+};
+
+enum ddr_command stm32mp1_get_command(char *cmd, int argc)
+{
+	const char *cmd_string[DDR_CMD_UNKNOWN] = {
+		[DDR_CMD_HELP] = "help",
+		[DDR_CMD_INFO] = "info",
+		[DDR_CMD_FREQ] = "freq",
+		[DDR_CMD_RESET] = "reset",
+		[DDR_CMD_PARAM] = "param",
+		[DDR_CMD_PRINT] = "print",
+		[DDR_CMD_EDIT] = "edit",
+		[DDR_CMD_STEP] = "step",
+		[DDR_CMD_NEXT] = "next",
+		[DDR_CMD_GO] = "go",
+		[DDR_CMD_TEST] = "test",
+#ifdef CONFIG_STM32MP1_DDR_TUNING
+		[DDR_CMD_TUNING] = "tuning",
+#endif
+	};
+	/* min and max number of argument */
+	const char cmd_arg[DDR_CMD_UNKNOWN][2] = {
+		[DDR_CMD_HELP] = { 0, 0 },
+		[DDR_CMD_INFO] = { 0, 255 },
+		[DDR_CMD_FREQ] = { 0, 1 },
+		[DDR_CMD_RESET] = { 0, 0 },
+		[DDR_CMD_PARAM] = { 0, 2 },
+		[DDR_CMD_PRINT] = { 0, 1 },
+		[DDR_CMD_EDIT] = { 2, 2 },
+		[DDR_CMD_STEP] = { 0, 1 },
+		[DDR_CMD_NEXT] = { 0, 0 },
+		[DDR_CMD_GO] = { 0, 0 },
+		[DDR_CMD_TEST] = { 0, 255 },
+#ifdef CONFIG_STM32MP1_DDR_TUNING
+		[DDR_CMD_TUNING] = { 0, 255 },
+#endif
+	};
+	int i;
+
+	for (i = 0; i < DDR_CMD_UNKNOWN; i++)
+		if (!strcmp(cmd, cmd_string[i])) {
+			if (argc - 1 < cmd_arg[i][0]) {
+				printf("no enought argument (min=%d)\n",
+				       cmd_arg[i][0]);
+				return DDR_CMD_UNKNOWN;
+			} else if (argc - 1 > cmd_arg[i][1]) {
+				printf("too many argument (max=%d)\n",
+				       cmd_arg[i][1]);
+				return DDR_CMD_UNKNOWN;
+			} else {
+				return i;
+			}
+		}
+
+	printf("unknown command %s\n", cmd);
+	return DDR_CMD_UNKNOWN;
+}
+
+static void stm32mp1_do_usage(void)
+{
+	const char *usage = {
+		"commands:\n\n"
+		"help                       this message\n"
+		"info  [<param> <val>]      display/change DDR information\n"
+		"freq  [freq]               display/change the DDR frequency\n"
+		"param [type|reg]           print input parameters\n"
+		"param <reg> <val>          edit parameters in step 0\n"
+		"print [type|reg]           dump register\n"
+		"edit <reg> <val>           modify register\n"
+		"       all registers if [type|reg] is absent\n"
+		"       <type> = ctl, phy\n"
+		"                or one category (static, timing, map, perf, cal, dyn)\n"
+		"       <reg> = name of the register\n"
+		"step [n]                   list the step / go to the step <n>\n"
+		"next                       go to the next step\n"
+		"go                         continue SPL execution\n"
+		"reset                      reboot machine\n"
+		"test [help] | <n> [...]    list (with help) or execute test <n>\n"
+#ifdef CONFIG_STM32MP1_DDR_TUNING
+		"tuning [help] | <n> [...] list (with help) or execute test <n>\n"
+#endif
+	};
+
+	puts(usage);
+}
+
+static bool stm32mp1_check_step(enum stm32mp1_ddr_interact_step step,
+				enum stm32mp1_ddr_interact_step expected)
+{
+	if (step != expected) {
+		printf("invalid step %d:%s expecting %d:%s\n",
+		       step, step_str[step],
+		       expected,
+		       step_str[expected]);
+		return false;
+	}
+	return true;
+}
+
+static void stm32mp1_do_info(struct ddr_info *priv,
+			     struct stm32mp1_ddr_config *config,
+			     enum stm32mp1_ddr_interact_step step,
+			     int argc, char * const argv[])
+{
+	unsigned long value;
+	static char *ddr_name;
+
+	if (argc == 1) {
+		printf("step = %d : %s\n", step, step_str[step]);
+		printf("name = %s\n", config->info.name);
+		printf("size = 0x%x\n", config->info.size);
+		printf("speed = %d kHz\n", config->info.speed);
+		return;
+	}
+
+	if (argc < 3) {
+		printf("no enought parameter\n");
+		return;
+	}
+	if (!strcmp(argv[1], "name")) {
+		u32 i, name_len = 0;
+
+		for (i = 2; i < argc; i++)
+			name_len += strlen(argv[i]) + 1;
+		if (ddr_name)
+			free(ddr_name);
+		ddr_name = malloc(name_len);
+		config->info.name = ddr_name;
+		if (!ddr_name) {
+			printf("alloc error, length %d\n", name_len);
+			return;
+		}
+		strcpy(ddr_name, argv[2]);
+		for (i = 3; i < argc; i++) {
+			strcat(ddr_name, " ");
+			strcat(ddr_name, argv[i]);
+		}
+		printf("name = %s\n", ddr_name);
+		return;
+	}
+	if (!strcmp(argv[1], "size")) {
+		if (strict_strtoul(argv[2], 16, &value) < 0) {
+			printf("invalid value %s\n", argv[2]);
+		} else {
+			config->info.size = value;
+			printf("size = 0x%x\n", config->info.size);
+		}
+		return;
+	}
+	if (!strcmp(argv[1], "speed")) {
+		if (strict_strtoul(argv[2], 10, &value) < 0) {
+			printf("invalid value %s\n", argv[2]);
+		} else {
+			config->info.speed = value;
+			printf("speed = %d kHz\n", config->info.speed);
+			value = clk_get_rate(&priv->clk);
+			printf("DDRPHY = %ld kHz\n", value / 1000);
+		}
+		return;
+	}
+	printf("argument %s invalid\n", argv[1]);
+}
+
+static bool stm32mp1_do_freq(struct ddr_info *priv,
+			     int argc, char * const argv[])
+{
+	unsigned long ddrphy_clk;
+
+	if (argc == 2) {
+		if (strict_strtoul(argv[1], 0, &ddrphy_clk) < 0) {
+			printf("invalid argument %s", argv[1]);
+			return false;
+		}
+		if (clk_set_rate(&priv->clk, ddrphy_clk * 1000)) {
+			printf("ERROR: update failed!\n");
+			return false;
+		}
+	}
+	ddrphy_clk = clk_get_rate(&priv->clk);
+	printf("DDRPHY = %ld kHz\n", ddrphy_clk / 1000);
+	if (argc == 2)
+		return true;
+	return false;
+}
+
+static void stm32mp1_do_param(enum stm32mp1_ddr_interact_step step,
+			      const struct stm32mp1_ddr_config *config,
+			      int argc, char * const argv[])
+{
+	switch (argc) {
+	case 1:
+		stm32mp1_dump_param(config, NULL);
+		break;
+	case 2:
+		if (stm32mp1_dump_param(config, argv[1]))
+			printf("invalid argument %s\n",
+			       argv[1]);
+		break;
+	case 3:
+		if (!stm32mp1_check_step(step, STEP_DDR_RESET))
+			return;
+		stm32mp1_edit_param(config, argv[1], argv[2]);
+		break;
+	}
+}
+
+static void stm32mp1_do_print(struct ddr_info *priv,
+			      int argc, char * const argv[])
+{
+	switch (argc) {
+	case 1:
+		stm32mp1_dump_reg(priv, NULL);
+		break;
+	case 2:
+		if (stm32mp1_dump_reg(priv, argv[1]))
+			printf("invalid argument %s\n",
+			       argv[1]);
+		break;
+	}
+}
+
+static int stm32mp1_do_step(enum stm32mp1_ddr_interact_step step,
+			    int argc, char * const argv[])
+{
+	int i;
+	unsigned long value;
+
+	switch (argc) {
+	case 1:
+		for (i = 0; i < ARRAY_SIZE(step_str); i++)
+			printf("%d:%s\n", i, step_str[i]);
+		break;
+
+	case 2:
+		if ((strict_strtoul(argv[1], 0,
+				    &value) < 0) ||
+				    value >= ARRAY_SIZE(step_str)) {
+			printf("invalid argument %s\n",
+			       argv[1]);
+			goto end;
+		}
+
+		if (value != STEP_DDR_RESET &&
+		    value <= step) {
+			printf("invalid target %d:%s, current step is %d:%s\n",
+			       (int)value, step_str[value],
+			       step, step_str[step]);
+			goto end;
+		}
+		printf("step to %d:%s\n",
+		       (int)value, step_str[value]);
+		return (int)value;
+	};
+
+end:
+	return step;
+}
+
+static const char * const s_result[] = {
+		[TEST_PASSED] = "Pass",
+		[TEST_FAILED] = "Failed",
+		[TEST_ERROR] = "Error"
+};
+
+static void stm32mp1_ddr_subcmd(struct ddr_info *priv,
+				int argc, char *argv[],
+				const struct test_desc array[],
+				const int array_nb)
+{
+	int i;
+	unsigned long value;
+	int result;
+	char string[50] = "";
+
+	if (argc == 1) {
+		printf("%s:%d\n", argv[0], array_nb);
+		for (i = 0; i < array_nb; i++)
+			printf("%d:%s:%s\n",
+			       i, array[i].name, array[i].usage);
+		return;
+	}
+	if (argc > 1 && !strcmp(argv[1], "help")) {
+		printf("%s:%d\n", argv[0], array_nb);
+		for (i = 0; i < array_nb; i++)
+			printf("%d:%s:%s:%s\n", i,
+			       array[i].name, array[i].usage, array[i].help);
+		return;
+	}
+
+	if ((strict_strtoul(argv[1], 0, &value) <  0) ||
+	    value >= array_nb) {
+		sprintf(string, "invalid argument %s",
+			argv[1]);
+		result = TEST_FAILED;
+		goto end;
+	}
+
+	if (argc > (array[value].max_args + 2)) {
+		sprintf(string, "invalid nb of args %d, max %d",
+			argc - 2, array[value].max_args);
+		result = TEST_FAILED;
+		goto end;
+	}
+
+	printf("execute %d:%s\n", (int)value, array[value].name);
+	clear_ctrlc();
+	result = array[value].fct(priv->ctl, priv->phy,
+				  string, argc - 2, &argv[2]);
+
+end:
+	printf("Result: %s [%s]\n", s_result[result], string);
+}
+
+bool stm32mp1_ddr_interactive(void *priv,
+			      enum stm32mp1_ddr_interact_step step,
+			      const struct stm32mp1_ddr_config *config)
+{
+	const char *prompt = "DDR>";
+	char buffer[CONFIG_SYS_CBSIZE];
+	char *argv[CONFIG_SYS_MAXARGS + 1];	/* NULL terminated */
+	int argc;
+	static int next_step = -1;
+
+	if (next_step < 0 && step == STEP_DDR_RESET) {
+#ifdef CONFIG_STM32MP1_DDR_INTERACTIVE_FORCE
+		gd->flags &= ~(GD_FLG_SILENT |
+			       GD_FLG_DISABLE_CONSOLE);
+		next_step = STEP_DDR_RESET;
+#else
+		unsigned long start = get_timer(0);
+
+		while (1) {
+			if (tstc() && (getc() == 'd')) {
+				next_step = STEP_DDR_RESET;
+				break;
+			}
+			if (get_timer(start) > 100)
+				break;
+		}
+#endif
+	}
+
+	debug("** step %d ** %s / %d\n", step, step_str[step], next_step);
+
+	if (next_step < 0)
+		return false;
+
+	if (step < 0 || step > ARRAY_SIZE(step_str)) {
+		printf("** step %d ** INVALID\n", step);
+		return false;
+	}
+
+	printf("%d:%s\n", step, step_str[step]);
+	printf("%s\n", prompt);
+
+	if (next_step > step)
+		return false;
+
+	while (next_step == step) {
+		cli_readline_into_buffer(prompt, buffer, 0);
+		argc = cli_simple_parse_line(buffer, argv);
+		if (!argc)
+			continue;
+
+		switch (stm32mp1_get_command(argv[0], argc)) {
+		case DDR_CMD_HELP:
+			stm32mp1_do_usage();
+			break;
+
+		case DDR_CMD_INFO:
+			stm32mp1_do_info(priv,
+					 (struct stm32mp1_ddr_config *)config,
+					 step, argc, argv);
+			break;
+
+		case DDR_CMD_FREQ:
+			if (stm32mp1_do_freq(priv, argc, argv))
+				next_step = STEP_DDR_RESET;
+			break;
+
+		case DDR_CMD_RESET:
+			do_reset(NULL, 0, 0, NULL);
+			break;
+
+		case DDR_CMD_PARAM:
+			stm32mp1_do_param(step, config, argc, argv);
+			break;
+
+		case DDR_CMD_PRINT:
+			stm32mp1_do_print(priv, argc, argv);
+			break;
+
+		case DDR_CMD_EDIT:
+			stm32mp1_edit_reg(priv, argv[1], argv[2]);
+			break;
+
+		case DDR_CMD_GO:
+			next_step = STEP_RUN;
+			break;
+
+		case DDR_CMD_NEXT:
+			next_step = step + 1;
+			break;
+
+		case DDR_CMD_STEP:
+			next_step = stm32mp1_do_step(step, argc, argv);
+			break;
+
+		case DDR_CMD_TEST:
+			if (!stm32mp1_check_step(step, STEP_DDR_READY))
+				continue;
+			stm32mp1_ddr_subcmd(priv, argc, argv, test, test_nb);
+			break;
+
+#ifdef CONFIG_STM32MP1_DDR_TUNING
+		case DDR_CMD_TUNING:
+			if (!stm32mp1_check_step(step, STEP_DDR_READY))
+				continue;
+			stm32mp1_ddr_subcmd(priv, argc, argv,
+					    tuning, tuning_nb);
+			break;
+#endif
+		default:
+			break;
+		}
+	}
+	return next_step == STEP_DDR_RESET;
+}
diff --git a/drivers/ram/stm32mp1/stm32mp1_ram.c b/drivers/ram/stm32mp1/stm32mp1_ram.c
index bd497a3..6245cb4 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ram.c
+++ b/drivers/ram/stm32mp1/stm32mp1_ram.c
@@ -12,6 +12,8 @@
 #include <asm/io.h>
 #include "stm32mp1_ddr.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 static const char *const clkname[] = {
 	"ddrc1",
 	"ddrc2",
@@ -20,7 +22,7 @@ static const char *const clkname[] = {
 	"ddrphyc" /* LAST clock => used for get_rate() */
 };
 
-int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint16_t mem_speed)
+int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint32_t mem_speed)
 {
 	unsigned long ddrphy_clk;
 	unsigned long ddr_clk;
@@ -43,16 +45,15 @@ int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint16_t mem_speed)
 	priv->clk = clk;
 	ddrphy_clk = clk_get_rate(&priv->clk);
 
-	debug("DDR: mem_speed (%d MHz), RCC %d MHz\n",
-	      mem_speed, (u32)(ddrphy_clk / 1000 / 1000));
+	debug("DDR: mem_speed (%d kHz), RCC %d kHz\n",
+	      mem_speed, (u32)(ddrphy_clk / 1000));
 	/* max 10% frequency delta */
-	ddr_clk = abs(ddrphy_clk - mem_speed * 1000 * 1000);
-	if (ddr_clk > (mem_speed * 1000 * 100)) {
-		pr_err("DDR expected freq %d MHz, current is %d MHz\n",
-		       mem_speed, (u32)(ddrphy_clk / 1000 / 1000));
-		return -EINVAL;
+	ddr_clk = abs(ddrphy_clk - mem_speed * 1000);
+	if (ddr_clk > (mem_speed * 100)) {
+		pr_err("DDR expected freq %d kHz, current is %d kHz\n",
+		       mem_speed, (u32)(ddrphy_clk / 1000));
+		return -1;
 	}
-
 	return 0;
 }
 
@@ -108,6 +109,7 @@ static __maybe_unused int stm32mp1_ddr_setup(struct udevice *dev)
 		}
 	}
 
+
 	ret = clk_get_by_name(dev, "axidcg", &axidcg);
 	if (ret) {
 		debug("%s: Cannot found axidcg\n", __func__);
@@ -141,7 +143,7 @@ static int stm32mp1_ddr_probe(struct udevice *dev)
 {
 	struct ddr_info *priv = dev_get_priv(dev);
 	struct regmap *map;
-	int ret;
+	int ret = 0;
 
 	debug("STM32MP1 DDR probe\n");
 	priv->dev = dev;
@@ -153,11 +155,17 @@ static int stm32mp1_ddr_probe(struct udevice *dev)
 	priv->ctl = regmap_get_range(map, 0);
 	priv->phy = regmap_get_range(map, 1);
 
+	map = syscon_get_regmap_by_driver_data(STM32MP_SYSCON_PWR);
+	if (IS_ERR(map))
+		return PTR_ERR(map);
+	priv->pwr = regmap_get_range(map, 0);
+
 	priv->rcc = STM32_RCC_BASE;
 
 	priv->info.base = STM32_DDR_BASE;
 
-#if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
+#if !defined(CONFIG_STM32MP1_TRUSTED) && \
+	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
 	priv->info.size = 0;
 	return stm32mp1_ddr_setup(dev);
 #else
diff --git a/drivers/ram/stm32mp1/stm32mp1_tests.c b/drivers/ram/stm32mp1/stm32mp1_tests.c
new file mode 100644
index 0000000..5f2af4e
--- /dev/null
+++ b/drivers/ram/stm32mp1/stm32mp1_tests.c
@@ -0,0 +1,1360 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+#include <common.h>
+#include <console.h>
+#include <asm/io.h>
+#include <linux/log2.h>
+#include "stm32mp1_tests.h"
+
+#define ADDR_INVALID	0xFFFFFFFF
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int get_bufsize(char *string, int argc, char *argv[], int arg_nb,
+		       size_t *bufsize, size_t default_size)
+{
+	unsigned long value;
+
+	if (argc > arg_nb) {
+		if (strict_strtoul(argv[arg_nb], 0, &value) < 0) {
+			sprintf(string, "invalid %d parameter %s",
+				arg_nb, argv[arg_nb]);
+			return -1;
+		}
+		if (value > STM32_DDR_SIZE || value == 0) {
+			sprintf(string, "invalid size %s", argv[arg_nb]);
+			return -1;
+		}
+		if (value & 0x3) {
+			sprintf(string, "unaligned size %s",
+				argv[arg_nb]);
+			return -1;
+		}
+		*bufsize = value;
+	} else {
+		if (default_size != STM32_DDR_SIZE)
+			*bufsize = default_size;
+		else
+			*bufsize = get_ram_size((long *)STM32_DDR_BASE,
+						STM32_DDR_SIZE);
+	}
+	return 0;
+}
+
+static int get_nb_loop(char *string, int argc, char *argv[], int arg_nb,
+		       u32 *nb_loop, u32 default_nb_loop)
+{
+	unsigned long value;
+
+	if (argc > arg_nb) {
+		if (strict_strtoul(argv[arg_nb], 0, &value) < 0) {
+			sprintf(string, "invalid %d parameter %s",
+				arg_nb, argv[arg_nb]);
+			return -1;
+		}
+		if (value == 0)
+			printf("WARNING: infinite loop requested\n");
+		*nb_loop = value;
+	} else {
+		*nb_loop = default_nb_loop;
+	}
+
+	return 0;
+}
+
+static int get_addr(char *string, int argc, char *argv[], int arg_nb,
+		    u32 *addr)
+{
+	unsigned long value;
+
+	if (argc > arg_nb) {
+		if (strict_strtoul(argv[arg_nb], 16, &value) < 0) {
+			sprintf(string, "invalid %d parameter %s",
+				arg_nb, argv[arg_nb]);
+			return -1;
+		}
+		if (value < STM32_DDR_BASE) {
+			sprintf(string, "too low address %s", argv[arg_nb]);
+			return -1;
+		}
+		if (value & 0x3 && value != ADDR_INVALID) {
+			sprintf(string, "unaligned address %s",
+				argv[arg_nb]);
+			return -1;
+		}
+		*addr = value;
+	} else {
+		*addr = STM32_DDR_BASE;
+	}
+
+	return 0;
+}
+
+static int get_pattern(char *string, int argc, char *argv[], int arg_nb,
+		       u32 *pattern, u32 default_pattern)
+{
+	unsigned long value;
+
+	if (argc > arg_nb) {
+		if (strict_strtoul(argv[arg_nb], 16, &value) < 0) {
+			sprintf(string, "invalid %d parameter %s",
+				arg_nb, argv[arg_nb]);
+			return -1;
+		}
+		*pattern = value;
+	} else {
+		*pattern = default_pattern;
+	}
+
+	return 0;
+}
+
+static u32 check_addr(u32 addr, u32 value)
+{
+	u32 data = readl(addr);
+
+	if (value !=  data) {
+		printf("0x%08x: 0x%08x <=> 0x%08x", addr, data, value);
+		data = readl(addr);
+		printf("(2nd read: 0x%08x)", data);
+		if (value == data)
+			printf("- read error");
+		else
+			printf("- write error");
+		printf("\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int progress(u32 offset)
+{
+	if (!(offset & 0xFFFFFF)) {
+		putc('.');
+		if (ctrlc()) {
+			printf("\ntest interrupted!\n");
+			return 1;
+		}
+	}
+	return 0;
+}
+
+static int test_loop_end(u32 *loop, u32 nb_loop, u32 progress)
+{
+	(*loop)++;
+	if (nb_loop && *loop >= nb_loop)
+		return 1;
+	if ((*loop) % progress)
+		return 0;
+	/* allow to interrupt the test only for progress step */
+	if (ctrlc()) {
+		printf("test interrupted!\n");
+		return 1;
+	}
+	printf("loop #%d\n", *loop);
+	return 0;
+}
+
+/**********************************************************************
+ *
+ * Function:    memTestDataBus()
+ *
+ * Description: Test the data bus wiring in a memory region by
+ *              performing a walking 1's test at a fixed address
+ *              within that region.  The address is selected
+ *              by the caller.
+ *
+ * Notes:
+ *
+ * Returns:     0 if the test succeeds.
+ *              A non-zero result is the first pattern that failed.
+ *
+ **********************************************************************/
+static u32 databus(u32 *address)
+{
+	u32 pattern;
+	u32 read_value;
+
+	/* Perform a walking 1's test at the given address. */
+	for (pattern = 1; pattern != 0; pattern <<= 1) {
+		/* Write the test pattern. */
+		writel(pattern, address);
+
+		/* Read it back (immediately is okay for this test). */
+		read_value = readl(address);
+		debug("%x: %x <=> %x\n",
+		      (u32)address, read_value, pattern);
+
+		if (read_value != pattern)
+			return pattern;
+	}
+
+	return 0;
+}
+
+/**********************************************************************
+ *
+ * Function:    memTestAddressBus()
+ *
+ * Description: Test the address bus wiring in a memory region by
+ *              performing a walking 1's test on the relevant bits
+ *              of the address and checking for aliasing. This test
+ *              will find single-bit address failures such as stuck
+ *              -high, stuck-low, and shorted pins.  The base address
+ *              and size of the region are selected by the caller.
+ *
+ * Notes:       For best results, the selected base address should
+ *              have enough LSB 0's to guarantee single address bit
+ *              changes.  For example, to test a 64-Kbyte region,
+ *              select a base address on a 64-Kbyte boundary.  Also,
+ *              select the region size as a power-of-two--if at all
+ *              possible.
+ *
+ * Returns:     NULL if the test succeeds.
+ *              A non-zero result is the first address at which an
+ *              aliasing problem was uncovered.  By examining the
+ *              contents of memory, it may be possible to gather
+ *              additional information about the problem.
+ *
+ **********************************************************************/
+static u32 *addressbus(u32 *address, u32 nb_bytes)
+{
+	u32 mask = (nb_bytes / sizeof(u32) - 1);
+	u32 offset;
+	u32 test_offset;
+	u32 read_value;
+
+	u32 pattern     = 0xAAAAAAAA;
+	u32 antipattern = 0x55555555;
+
+	/* Write the default pattern at each of the power-of-two offsets. */
+	for (offset = 1; (offset & mask) != 0; offset <<= 1)
+		writel(pattern, &address[offset]);
+
+	/* Check for address bits stuck high. */
+	test_offset = 0;
+	writel(antipattern, &address[test_offset]);
+
+	for (offset = 1; (offset & mask) != 0; offset <<= 1) {
+		read_value = readl(&address[offset]);
+		debug("%x: %x <=> %x\n",
+		      (u32)&address[offset], read_value, pattern);
+		if (read_value != pattern)
+			return &address[offset];
+	}
+
+	writel(pattern, &address[test_offset]);
+
+	/* Check for address bits stuck low or shorted. */
+	for (test_offset = 1; (test_offset & mask) != 0; test_offset <<= 1) {
+		writel(antipattern, &address[test_offset]);
+		if (readl(&address[0]) != pattern)
+			return &address[test_offset];
+
+		for (offset = 1; (offset & mask) != 0; offset <<= 1) {
+			if (readl(&address[offset]) != pattern &&
+			    offset != test_offset)
+				return &address[test_offset];
+		}
+		writel(pattern, &address[test_offset]);
+	}
+
+	return NULL;
+}
+
+/**********************************************************************
+ *
+ * Function:    memTestDevice()
+ *
+ * Description: Test the integrity of a physical memory device by
+ *              performing an increment/decrement test over the
+ *              entire region.  In the process every storage bit
+ *              in the device is tested as a zero and a one.  The
+ *              base address and the size of the region are
+ *              selected by the caller.
+ *
+ * Notes:
+ *
+ * Returns:     NULL if the test succeeds.
+ *
+ *              A non-zero result is the first address at which an
+ *              incorrect value was read back.  By examining the
+ *              contents of memory, it may be possible to gather
+ *              additional information about the problem.
+ *
+ **********************************************************************/
+static u32 *memdevice(u32 *address, u32 nb_bytes)
+{
+	u32 offset;
+	u32 nb_words = nb_bytes / sizeof(u32);
+
+	u32 pattern;
+	u32 antipattern;
+
+	puts("Fill with pattern");
+	/* Fill memory with a known pattern. */
+	for (pattern = 1, offset = 0; offset < nb_words; pattern++, offset++) {
+		writel(pattern, &address[offset]);
+		if (progress(offset))
+			return NULL;
+	}
+
+	puts("\nCheck and invert pattern");
+	/* Check each location and invert it for the second pass. */
+	for (pattern = 1, offset = 0; offset < nb_words; pattern++, offset++) {
+		if (readl(&address[offset]) != pattern)
+			return &address[offset];
+
+		antipattern = ~pattern;
+		writel(antipattern, &address[offset]);
+		if (progress(offset))
+			return NULL;
+	}
+
+	puts("\nCheck inverted pattern");
+	/* Check each location for the inverted pattern and zero it. */
+	for (pattern = 1, offset = 0; offset < nb_words; pattern++, offset++) {
+		antipattern = ~pattern;
+		if (readl(&address[offset]) != antipattern)
+			return &address[offset];
+		if (progress(offset))
+			return NULL;
+	}
+	printf("\n");
+
+	return NULL;
+}
+
+static enum test_result databuswalk0(struct stm32mp1_ddrctl *ctl,
+				     struct stm32mp1_ddrphy *phy,
+				     char *string, int argc, char *argv[])
+{
+	int i;
+	u32 loop = 0, nb_loop;
+	u32 addr;
+	u32 error = 0;
+	u32 data;
+
+	if (get_nb_loop(string, argc, argv, 0, &nb_loop, 100))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+
+	printf("running %d loops at 0x%x\n", nb_loop, addr);
+	while (!error) {
+		for (i = 0; i < 32; i++)
+			writel(~(1 << i), addr + 4 * i);
+		for (i = 0; i < 32; i++) {
+			data = readl(addr + 4 * i);
+			if (~(1 << i) !=  data) {
+				error |= 1 << i;
+				debug("%x: error %x expected %x => error:%x\n",
+				      addr + 4 * i, data, ~(1 << i), error);
+			}
+		}
+		if (test_loop_end(&loop, nb_loop, 1000))
+			break;
+		for (i = 0; i < 32; i++)
+			writel(0, addr + 4 * i);
+	}
+	if (error) {
+		sprintf(string, "loop %d: error for bits 0x%x",
+			loop, error);
+		return TEST_FAILED;
+	}
+	sprintf(string, "no error for %d loops", loop);
+	return TEST_PASSED;
+}
+
+static enum test_result databuswalk1(struct stm32mp1_ddrctl *ctl,
+				     struct stm32mp1_ddrphy *phy,
+				     char *string, int argc, char *argv[])
+{
+	int i;
+	u32 loop = 0, nb_loop;
+	u32 addr;
+	u32 error = 0;
+	u32 data;
+
+	if (get_nb_loop(string, argc, argv, 0, &nb_loop, 100))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+	printf("running %d loops at 0x%x\n", nb_loop, addr);
+	while (!error) {
+		for (i = 0; i < 32; i++)
+			writel(1 << i, addr + 4 * i);
+		for (i = 0; i < 32; i++) {
+			data = readl(addr + 4 * i);
+			if ((1 << i) !=  data) {
+				error |= 1 << i;
+				debug("%x: error %x expected %x => error:%x\n",
+				      addr + 4 * i, data, (1 << i), error);
+			}
+		}
+		if (test_loop_end(&loop, nb_loop, 1000))
+			break;
+		for (i = 0; i < 32; i++)
+			writel(0, addr + 4 * i);
+	}
+	if (error) {
+		sprintf(string, "loop %d: error for bits 0x%x",
+			loop, error);
+		return TEST_FAILED;
+	}
+	sprintf(string, "no error for %d loops", loop);
+	return TEST_PASSED;
+}
+
+static enum test_result test_databus(struct stm32mp1_ddrctl *ctl,
+				     struct stm32mp1_ddrphy *phy,
+				     char *string, int argc, char *argv[])
+{
+	u32 addr;
+	u32 error;
+
+	if (get_addr(string, argc, argv, 0, &addr))
+		return TEST_ERROR;
+	error = databus((u32 *)addr);
+	if (error) {
+		sprintf(string, "0x%x: error for bits 0x%x",
+			addr, error);
+		return TEST_FAILED;
+	}
+	sprintf(string, "address 0x%x", addr);
+	return TEST_PASSED;
+}
+
+static enum test_result test_addressbus(struct stm32mp1_ddrctl *ctl,
+					struct stm32mp1_ddrphy *phy,
+					char *string, int argc, char *argv[])
+{
+	u32 addr;
+	u32 bufsize;
+	u32 error;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+	if (!is_power_of_2(bufsize)) {
+		sprintf(string, "size 0x%x is not a power of 2",
+			(u32)bufsize);
+		return TEST_ERROR;
+	}
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+
+	error = (u32)addressbus((u32 *)addr, bufsize);
+	if (error) {
+		sprintf(string, "0x%x: error for address 0x%x",
+			addr, error);
+		return TEST_FAILED;
+	}
+	sprintf(string, "address 0x%x, size 0x%x",
+		addr, bufsize);
+	return TEST_PASSED;
+}
+
+static enum test_result test_memdevice(struct stm32mp1_ddrctl *ctl,
+				       struct stm32mp1_ddrphy *phy,
+				       char *string, int argc, char *argv[])
+{
+	u32 addr;
+	size_t bufsize;
+	u32 error;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+	error = (u32)memdevice((u32 *)addr, (unsigned long)bufsize);
+	if (error) {
+		sprintf(string, "0x%x: error for address 0x%x",
+			addr, error);
+		return TEST_FAILED;
+	}
+	sprintf(string, "address 0x%x, size 0x%x",
+		addr, bufsize);
+	return TEST_PASSED;
+}
+
+/**********************************************************************
+ *
+ * Function:    sso
+ *
+ * Description: Test the Simultaneous Switching Output.
+ *              Verifies succes sive reads and writes to the same memory word,
+ *              holding one bit constant while toggling all other data bits
+ *              simultaneously
+ *              => stress the data bus over an address range
+ *
+ *              The CPU writes to each address in the given range.
+ *              For each bit, first the CPU holds the bit at 1 while
+ *              toggling the other bits, and then the CPU holds the bit at 0
+ *              while toggling the other bits.
+ *              After each write, the CPU reads the address that was written
+ *              to verify that it contains the correct data
+ *
+ **********************************************************************/
+static enum test_result test_sso(struct stm32mp1_ddrctl *ctl,
+				 struct stm32mp1_ddrphy *phy,
+				 char *string, int argc, char *argv[])
+{
+	int i, j;
+	u32 addr, bufsize, remaining, offset;
+	u32 error = 0;
+	u32 data;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+
+	printf("running sso at 0x%x length 0x%x", addr, bufsize);
+	offset = addr;
+	remaining = bufsize;
+	while (remaining) {
+		for (i = 0; i < 32; i++) {
+			/* write pattern. */
+			for (j = 0; j < 6; j++) {
+				switch (j) {
+				case 0:
+				case 2:
+					data = 1 << i;
+					break;
+				case 3:
+				case 5:
+					data = ~(1 << i);
+					break;
+				case 1:
+					data = ~0x0;
+					break;
+				case 4:
+					data = 0x0;
+					break;
+				}
+
+				writel(data, offset);
+				error = check_addr(offset, data);
+				if (error)
+					goto end;
+			}
+		}
+		offset += 4;
+		remaining -= 4;
+		if (progress(offset << 7))
+			goto end;
+	}
+	puts("\n");
+
+end:
+	if (error) {
+		sprintf(string, "error for pattern 0x%x @0x%x",
+			data, offset);
+		return TEST_FAILED;
+	}
+	sprintf(string, "no error for sso at 0x%x length 0x%x", addr, bufsize);
+	return TEST_PASSED;
+}
+
+/**********************************************************************
+ *
+ * Function:    Random
+ *
+ * Description: Verifies r/w with pseudo-ramdom value on one region
+ *              + write the region (individual access)
+ *              + memcopy to the 2nd region (try to use burst)
+ *              + verify the 2 regions
+ *
+ **********************************************************************/
+static enum test_result test_random(struct stm32mp1_ddrctl *ctl,
+				    struct stm32mp1_ddrphy *phy,
+				    char *string, int argc, char *argv[])
+{
+	u32 addr, offset, value = 0;
+	size_t bufsize;
+	u32 loop = 0, nb_loop;
+	u32 error = 0;
+	unsigned int seed;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+	if (get_nb_loop(string, argc, argv, 1, &nb_loop, 1))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 2, &addr))
+		return TEST_ERROR;
+
+	printf("running %d loops at 0x%x\n", nb_loop, addr);
+	while (!error) {
+		seed = rand();
+		for (offset = addr; offset < addr + bufsize; offset += 4)
+			writel(rand(), offset);
+
+		memcpy((void *)addr + bufsize, (void *)addr, bufsize);
+
+		srand(seed);
+		for (offset = addr; offset < addr + 2 * bufsize; offset += 4) {
+			if (offset == (addr + bufsize))
+				srand(seed);
+			value = rand();
+			error = check_addr(offset, value);
+			if (error)
+				break;
+			if (progress(offset))
+				return TEST_FAILED;
+		}
+		if (test_loop_end(&loop, nb_loop, 100))
+			break;
+	}
+
+	if (error) {
+		sprintf(string,
+			"loop %d: error for address 0x%x: 0x%x expected 0x%x",
+			loop, offset, readl(offset), value);
+		return TEST_FAILED;
+	}
+	sprintf(string, "no error for %d loops, size 0x%x",
+		loop, bufsize);
+	return TEST_PASSED;
+}
+
+/**********************************************************************
+ *
+ * Function:    noise
+ *
+ * Description: Verifies r/w while forcing switching of all data bus lines.
+ *              optimised 4 iteration write/read/write/read cycles...
+ *              for pattern and inversed pattern
+ *
+ **********************************************************************/
+void do_noise(u32 addr, u32 pattern, u32 *result)
+{
+	__asm__("push {R0-R11}");
+	__asm__("mov r0, %0" : : "r" (addr));
+	__asm__("mov r1, %0" : : "r" (pattern));
+	__asm__("mov r11, %0" : : "r" (result));
+
+	__asm__("mvn r2, r1");
+
+	__asm__("str r1, [r0]");
+	__asm__("ldr r3, [r0]");
+	__asm__("str r2, [r0]");
+	__asm__("ldr r4, [r0]");
+
+	__asm__("str r1, [r0]");
+	__asm__("ldr r5, [r0]");
+	__asm__("str r2, [r0]");
+	__asm__("ldr r6, [r0]");
+
+	__asm__("str r1, [r0]");
+	__asm__("ldr r7, [r0]");
+	__asm__("str r2, [r0]");
+	__asm__("ldr r8, [r0]");
+
+	__asm__("str r1, [r0]");
+	__asm__("ldr r9, [r0]");
+	__asm__("str r2, [r0]");
+	__asm__("ldr r10, [r0]");
+
+	__asm__("stmia R11!, {R3-R10}");
+
+	__asm__("pop {R0-R11}");
+}
+
+static enum test_result test_noise(struct stm32mp1_ddrctl *ctl,
+				   struct stm32mp1_ddrphy *phy,
+				   char *string, int argc, char *argv[])
+{
+	u32 addr, pattern;
+	u32 result[8];
+	int i;
+	enum test_result res = TEST_PASSED;
+
+	if (get_pattern(string, argc, argv, 0, &pattern, 0xFFFFFFFF))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 1, &addr))
+		return TEST_ERROR;
+
+	printf("running noise for 0x%x at 0x%x\n", pattern, addr);
+
+	do_noise(addr, pattern, result);
+
+	for (i = 0; i < 0x8;) {
+		if (check_addr((u32)&result[i++], pattern))
+			res = TEST_FAILED;
+		if (check_addr((u32)&result[i++], ~pattern))
+			res = TEST_FAILED;
+	}
+
+	return res;
+}
+
+/**********************************************************************
+ *
+ * Function:    noise_burst
+ *
+ * Description: Verifies r/w while forcing switching of all data bus lines.
+ *              optimised write loop witrh store multiple to use burst
+ *              for pattern and inversed pattern
+ *
+ **********************************************************************/
+void do_noise_burst(u32 addr, u32 pattern, size_t bufsize)
+{
+	__asm__("push {R0-R9}");
+	__asm__("mov r0, %0" : : "r" (addr));
+	__asm__("mov r1, %0" : : "r" (pattern));
+	__asm__("mov r9, %0" : : "r" (bufsize));
+
+	__asm__("mvn r2, r1");
+	__asm__("mov r3, r1");
+	__asm__("mov r4, r2");
+	__asm__("mov r5, r1");
+	__asm__("mov r6, r2");
+	__asm__("mov r7, r1");
+	__asm__("mov r8, r2");
+
+	__asm__("loop1:");
+	__asm__("stmia R0!, {R1-R8}");
+	__asm__("stmia R0!, {R1-R8}");
+	__asm__("stmia R0!, {R1-R8}");
+	__asm__("stmia R0!, {R1-R8}");
+	__asm__("subs r9, r9, #128");
+	__asm__("bge loop1");
+	__asm__("pop {R0-R9}");
+}
+
+/* chunk size enough to allow interruption with Ctrl-C*/
+#define CHUNK_SIZE	0x8000000
+static enum test_result test_noise_burst(struct stm32mp1_ddrctl *ctl,
+					 struct stm32mp1_ddrphy *phy,
+					 char *string, int argc, char *argv[])
+{
+	u32 addr, offset, pattern;
+	size_t bufsize, remaining, size;
+	int i;
+	enum test_result res = TEST_PASSED;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+	if (get_pattern(string, argc, argv, 1, &pattern, 0xFFFFFFFF))
+		return TEST_ERROR;
+	if (get_addr(string, argc, argv, 2, &addr))
+		return TEST_ERROR;
+
+	printf("running noise burst for 0x%x at 0x%x + 0x%x",
+	       pattern, addr, bufsize);
+
+	offset = addr;
+	remaining = bufsize;
+	size = CHUNK_SIZE;
+	while (remaining) {
+		if (remaining < size)
+			size = remaining;
+		do_noise_burst(offset, pattern, size);
+		remaining -= size;
+		offset += size;
+		if (progress(offset)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+	puts("\ncheck buffer");
+	for (i = 0; i < bufsize;) {
+		if (check_addr(addr + i, pattern))
+			res = TEST_FAILED;
+		i += 4;
+		if (check_addr(addr + i, ~pattern))
+			res = TEST_FAILED;
+		i += 4;
+		if (progress(i)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+end:
+	puts("\n");
+	return res;
+}
+
+/**********************************************************************
+ *
+ * Function:    pattern test
+ *
+ * Description: optimized loop for read/write pattern (array of 8 u32)
+ *
+ **********************************************************************/
+#define PATTERN_SIZE	8
+static enum test_result test_loop(const u32 *pattern, u32 *address,
+				  const u32 bufsize)
+{
+	int i;
+	int j;
+	enum test_result res = TEST_PASSED;
+	u32 *offset, testsize, remaining;
+
+	offset = address;
+	remaining = bufsize;
+	while (remaining) {
+		testsize = bufsize > 0x1000000 ? 0x1000000 : bufsize;
+
+		__asm__("push {R0-R10}");
+		__asm__("mov r0, %0" : : "r" (pattern));
+		__asm__("mov r1, %0" : : "r" (offset));
+		__asm__("mov r2, %0" : : "r" (testsize));
+		__asm__("ldmia r0!, {R3-R10}");
+
+		__asm__("loop2:");
+		__asm__("stmia r1!, {R3-R10}");
+		__asm__("stmia r1!, {R3-R10}");
+		__asm__("stmia r1!, {R3-R10}");
+		__asm__("stmia r1!, {R3-R10}");
+		__asm__("subs r2, r2, #8");
+		__asm__("bge loop2");
+		__asm__("pop {R0-R10}");
+
+		offset += testsize;
+		remaining -= testsize;
+		if (progress((u32)offset)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+
+	puts("\ncheck buffer");
+	for (i = 0; i < bufsize; i += PATTERN_SIZE * 4) {
+		for (j = 0; j < PATTERN_SIZE; j++, address++)
+			if (check_addr((u32)address, pattern[j])) {
+				res = TEST_FAILED;
+				goto end;
+			}
+		if (progress(i)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+
+end:
+	puts("\n");
+	return res;
+}
+
+const u32 pattern_div1_x16[PATTERN_SIZE] = {
+	0x0000FFFF, 0x0000FFFF, 0x0000FFFF, 0x0000FFFF,
+	0x0000FFFF, 0x0000FFFF, 0x0000FFFF, 0x0000FFFF
+};
+
+const u32 pattern_div2_x16[PATTERN_SIZE] = {
+	0xFFFFFFFF, 0x00000000, 0xFFFFFFFF, 0x00000000,
+	0xFFFFFFFF, 0x00000000, 0xFFFFFFFF, 0x00000000
+};
+
+const u32 pattern_div4_x16[PATTERN_SIZE] = {
+	0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000,
+	0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000
+};
+
+const u32 pattern_div4_x32[PATTERN_SIZE] = {
+	0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000
+};
+
+const u32 pattern_mostly_zero_x16[PATTERN_SIZE] = {
+	0x00000000, 0x00000000, 0x00000000, 0x0000FFFF,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000
+};
+
+const u32 pattern_mostly_zero_x32[PATTERN_SIZE] = {
+	0x00000000, 0x00000000, 0x00000000, 0xFFFFFFFF,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000
+};
+
+const u32 pattern_mostly_one_x16[PATTERN_SIZE] = {
+	0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0000FFFF,
+	0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF
+};
+
+const u32 pattern_mostly_one_x32[PATTERN_SIZE] = {
+	0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000,
+	0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF
+};
+
+#define NB_PATTERN	5
+static enum test_result test_freq_pattern(struct stm32mp1_ddrctl *ctl,
+					  struct stm32mp1_ddrphy *phy,
+					  char *string, int argc, char *argv[])
+{
+	const u32 * const patterns_x16[NB_PATTERN] = {
+		pattern_div1_x16,
+		pattern_div2_x16,
+		pattern_div4_x16,
+		pattern_mostly_zero_x16,
+		pattern_mostly_one_x16,
+	};
+	const u32 * const patterns_x32[NB_PATTERN] = {
+		pattern_div2_x16,
+		pattern_div4_x16,
+		pattern_div4_x32,
+		pattern_mostly_zero_x32,
+		pattern_mostly_one_x32
+	};
+	const char *patterns_comments[NB_PATTERN] = {
+		"switching at frequency F/1",
+		"switching at frequency F/2",
+		"switching at frequency F/4",
+		"mostly zero",
+		"mostly one"
+	};
+
+	enum test_result res = TEST_PASSED, pattern_res;
+	int i, bus_width;
+	const u32 **patterns;
+	u32 bufsize;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	switch (readl(&ctl->mstr) & DDRCTRL_MSTR_DATA_BUS_WIDTH_MASK) {
+	case DDRCTRL_MSTR_DATA_BUS_WIDTH_HALF:
+	case DDRCTRL_MSTR_DATA_BUS_WIDTH_QUARTER:
+		bus_width = 16;
+		break;
+	default:
+		bus_width = 32;
+		break;
+	}
+
+	printf("running test pattern at 0x%08x length 0x%x width = %d\n",
+	       STM32_DDR_BASE, bufsize, bus_width);
+
+	patterns =
+		(const u32 **)(bus_width == 16 ? patterns_x16 : patterns_x32);
+
+	for (i = 0; i < NB_PATTERN; i++) {
+		printf("test data pattern %s:", patterns_comments[i]);
+		pattern_res = test_loop(patterns[i], (u32 *)STM32_DDR_BASE,
+					bufsize);
+		if (pattern_res != TEST_PASSED)	{
+			printf("Failed\n");
+			return pattern_res;
+		}
+		printf("Passed\n");
+	}
+
+	return res;
+}
+
+/**********************************************************************
+ *
+ * Function:    pattern test with size
+ *
+ * Description: loop for write pattern
+ *
+ **********************************************************************/
+
+static enum test_result test_loop_size(const u32 *pattern, u32 size,
+				       u32 *address,
+				       const u32 bufsize)
+{
+	int i, j;
+	enum test_result res = TEST_PASSED;
+	u32 *p = address;
+
+	for (i = 0; i < bufsize; i += size * 4) {
+		for (j = 0; j < size ; j++, p++)
+			*p = pattern[j];
+		if (progress(i)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+
+	puts("\ncheck buffer");
+	p = address;
+	for (i = 0; i < bufsize; i += size * 4) {
+		for (j = 0; j < size; j++, p++)
+			if (check_addr((u32)p, pattern[j])) {
+				res = TEST_FAILED;
+				goto end;
+			}
+		if (progress(i)) {
+			res = TEST_FAILED;
+			goto end;
+		}
+	}
+
+end:
+	puts("\n");
+	return res;
+}
+
+static enum test_result test_checkboard(struct stm32mp1_ddrctl *ctl,
+					struct stm32mp1_ddrphy *phy,
+					char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+
+	u32 checkboard[2] = {0x55555555, 0xAAAAAAAA};
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running test checkboard at 0x%08x length 0x%x\n",
+	       STM32_DDR_BASE, bufsize);
+
+	res = test_loop_size(checkboard, 2, (u32 *)STM32_DDR_BASE,
+			     bufsize);
+
+	checkboard[0] = ~checkboard[0];
+	checkboard[1] = ~checkboard[1];
+
+	res = test_loop_size(checkboard, 2, (u32 *)STM32_DDR_BASE,
+			     bufsize);
+
+	return res;
+}
+
+static enum test_result test_blockseq(struct stm32mp1_ddrctl *ctl,
+				      struct stm32mp1_ddrphy *phy,
+				      char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+	int i;
+
+	u32 value;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running at 0x%08x length 0x%x\n", STM32_DDR_BASE, bufsize);
+	for (i = 0; i < 256; i++) {
+		value = i | i << 8 | i << 16 | i << 24;
+		printf("pattern = %08x", value);
+		res = test_loop_size(&value, 1, (u32 *)STM32_DDR_BASE,
+				     bufsize);
+		if (res != TEST_PASSED)
+			return res;
+	}
+
+	return res;
+}
+
+static enum test_result test_walkbit0(struct stm32mp1_ddrctl *ctl,
+				      struct stm32mp1_ddrphy *phy,
+				      char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+	int i;
+
+	u32 value;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running at 0x%08x length 0x%x\n", STM32_DDR_BASE, bufsize);
+	for (i = 0; i < 64; i++) {
+		if (i < 32)
+			value = 1 << i;
+		else
+			value = 1 << (63 - i);
+
+		printf("pattern = %08x", value);
+		res = test_loop_size(&value, 1, (u32 *)STM32_DDR_BASE,
+				     bufsize);
+		if (res != TEST_PASSED)
+			return res;
+	}
+
+	return res;
+}
+
+static enum test_result test_walkbit1(struct stm32mp1_ddrctl *ctl,
+				      struct stm32mp1_ddrphy *phy,
+				      char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+	int i;
+
+	u32 value;
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running at 0x%08x length 0x%x\n", STM32_DDR_BASE, bufsize);
+	for (i = 0; i < 64; i++) {
+		if (i < 32)
+			value = ~(1 << i);
+		else
+			value = ~(1 << (63 - i));
+
+		printf("pattern = %08x", value);
+		res = test_loop_size(&value, 1, (u32 *)STM32_DDR_BASE,
+				     bufsize);
+		if (res != TEST_PASSED)
+			return res;
+	}
+
+	return res;
+}
+
+/*
+ * try to catch bad bits which are dependent on the current values of
+ * surrounding bits in either the same word32
+ */
+static enum test_result test_bitspread(struct stm32mp1_ddrctl *ctl,
+				       struct stm32mp1_ddrphy *phy,
+				       char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+	int i, j;
+
+	u32 bitspread[4];
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running at 0x%08x length 0x%x\n", STM32_DDR_BASE, bufsize);
+	for (i = 1; i < 32; i++) {
+		for (j = 0; j < i; j++) {
+			if (i < 32)
+				bitspread[0] = (1 << i) | (1 << j);
+			else
+				bitspread[0] = (1 << (63 - i)) |
+					       (1 << (63 - j));
+			bitspread[1] = bitspread[0];
+			bitspread[2] = ~bitspread[0];
+			bitspread[3] = ~bitspread[0];
+			printf("pattern = %08x", bitspread[0]);
+
+			res = test_loop_size(bitspread, 4,
+					     (u32 *)STM32_DDR_BASE,
+					     bufsize);
+			if (res != TEST_PASSED)
+				return res;
+		}
+	}
+
+	return res;
+}
+
+static enum test_result test_bitflip(struct stm32mp1_ddrctl *ctl,
+				     struct stm32mp1_ddrphy *phy,
+				     char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED;
+	u32 bufsize;
+	int i;
+
+	u32 bitflip[4];
+
+	if (get_bufsize(string, argc, argv, 0, &bufsize, 4 * 1024))
+		return TEST_ERROR;
+
+	printf("running at 0x%08x length 0x%x\n", STM32_DDR_BASE, bufsize);
+	for (i = 0; i < 32; i++) {
+		bitflip[0] = 1 << i;
+		bitflip[1] = bitflip[0];
+		bitflip[2] = ~bitflip[0];
+		bitflip[3] = bitflip[2];
+		printf("pattern = %08x", bitflip[0]);
+
+		res = test_loop_size(bitflip, 4, (u32 *)STM32_DDR_BASE,
+				     bufsize);
+		if (res != TEST_PASSED)
+			return res;
+	}
+
+	return res;
+}
+
+/**********************************************************************
+ *
+ * Function: infinite read access to DDR
+ *
+ * Description: continuous read the same pattern at the same address
+ *
+ **********************************************************************/
+static enum test_result test_read(struct stm32mp1_ddrctl *ctl,
+				  struct stm32mp1_ddrphy *phy,
+				  char *string, int argc, char *argv[])
+{
+	u32 *addr;
+	u32 data;
+	u32 loop = 0;
+	bool random = false;
+
+	if (get_addr(string, argc, argv, 0, (u32 *)&addr))
+		return TEST_ERROR;
+
+	if ((u32)addr == ADDR_INVALID) {
+		printf("random ");
+		random = true;
+	}
+
+	printf("running at 0x%08x\n", (u32)addr);
+
+	while (1) {
+		if (random)
+			addr = (u32 *)(STM32_DDR_BASE +
+			       (rand() & (STM32_DDR_SIZE - 1) & ~0x3));
+		data = readl(addr);
+		if (test_loop_end(&loop, 0, 1000))
+			break;
+	}
+	sprintf(string, "0x%x: %x", (u32)addr, data);
+
+	return TEST_PASSED;
+}
+
+/**********************************************************************
+ *
+ * Function: infinite write access to DDR
+ *
+ * Description: continuous write the same pattern at the same address
+ *
+ **********************************************************************/
+static enum test_result test_write(struct stm32mp1_ddrctl *ctl,
+				   struct stm32mp1_ddrphy *phy,
+				   char *string, int argc, char *argv[])
+{
+	u32 *addr;
+	u32 data = 0xA5A5AA55;
+	u32 loop = 0;
+	bool random = false;
+
+	if (get_addr(string, argc, argv, 0, (u32 *)&addr))
+		return TEST_ERROR;
+
+	if ((u32)addr == ADDR_INVALID) {
+		printf("random ");
+		random = true;
+	}
+
+	printf("running at 0x%08x\n", (u32)addr);
+
+	while (1) {
+		if (random) {
+			addr = (u32 *)(STM32_DDR_BASE +
+			       (rand() & (STM32_DDR_SIZE - 1) & ~0x3));
+			data = rand();
+		}
+		writel(data, addr);
+		if (test_loop_end(&loop, 0, 1000))
+			break;
+	}
+	sprintf(string, "0x%x: %x", (u32)addr, data);
+
+	return TEST_PASSED;
+}
+
+#define NB_TEST_INFINITE 2
+static enum test_result test_all(struct stm32mp1_ddrctl *ctl,
+				 struct stm32mp1_ddrphy *phy,
+				 char *string, int argc, char *argv[])
+{
+	enum test_result res = TEST_PASSED, result;
+	int i, nb_error = 0;
+
+	/* execute all the test except the lasts which are infinite */
+	for (i = 1; i < test_nb - NB_TEST_INFINITE; i++) {
+		printf("execute %d:%s\n", (int)i, test[i].name);
+		result = test[i].fct(ctl, phy, string, 0, NULL);
+		printf("result %d:%s = ", (int)i, test[i].name);
+		if (result != TEST_PASSED) {
+			nb_error++;
+			res = TEST_FAILED;
+			puts("Failed");
+		} else {
+			puts("Passed");
+		}
+		puts("\n\n");
+	}
+	sprintf(string, "%d/%d test failed", nb_error,
+		test_nb - NB_TEST_INFINITE);
+
+	return res;
+}
+
+/****************************************************************
+ * TEST Description
+ ****************************************************************/
+
+const struct test_desc test[] = {
+	{test_all, "All", "", "Execute all tests", 0 },
+	{test_databus, "Simple DataBus", "[addr]",
+	 "Verifies each data line by walking 1 on fixed address",
+	 1
+	 },
+	{databuswalk0, "DataBusWalking0", "[loop] [addr]",
+	 "Verifies each data bus signal can be driven low (32 word burst)",
+	 2
+	},
+	{databuswalk1, "DataBusWalking1", "[loop] [addr]",
+	 "Verifies each data bus signal can be driven high (32 word burst)",
+	 2
+	},
+	{test_addressbus, "AddressBus", "[size] [addr]",
+	 "Verifies each relevant bits of the address and checking for aliasing",
+	 2
+	 },
+	{test_memdevice, "MemDevice", "[size] [addr]",
+	 "Test the integrity of a physical memory (test every storage bit in the region)",
+	 2
+	 },
+	{test_sso, "SimultaneousSwitchingOutput", "[size] [addr] ",
+	 "Stress the data bus over an address range",
+	 2
+	},
+	{test_noise, "Noise", "[pattern] [addr]",
+	 "Verifies r/w while forcing switching of all data bus lines.",
+	 3
+	},
+	{test_noise_burst, "NoiseBurst", "[size] [pattern] [addr]",
+	 "burst transfers while forcing switching of the data bus lines",
+	 3
+	},
+	{test_random, "Random", "[size] [loop] [addr]",
+	 "Verifies r/w and memcopy(burst for pseudo random value.",
+	 3
+	},
+	{test_freq_pattern, "FrequencySelectivePattern ", "[size]",
+	 "write & test patterns: Mostly Zero, Mostly One and F/n",
+	 1
+	},
+	{test_blockseq, "BlockSequential", "[size]",
+	 "test incremental pattern",
+	 1
+	},
+	{test_checkboard, "Checkerboard", "[size]",
+	 "test checker pattern",
+	 1
+	},
+	{test_bitspread, "BitSpread", "[size]",
+	 "test Bit Spread pattern",
+	 1
+	},
+	{test_bitflip, "BitFlip", "[size]",
+	 "test Bit Flip pattern",
+	 1
+	},
+	{test_walkbit0, "WalkingOnes", "[size]",
+	 "test Walking Ones pattern",
+	 1
+	},
+	{test_walkbit1, "WalkingZeroes", "[size]",
+	 "test Walking Zeroes pattern",
+	 1
+	},
+	/* need to the the 2 last one (infinite) : skipped for test all */
+	{test_read, "infinite read", "[addr]",
+	 "basic test : infinite read access", 1},
+	{test_write, "infinite write", "[addr]",
+	 "basic test : infinite write access", 1},
+};
+
+const int test_nb = ARRAY_SIZE(test);
diff --git a/drivers/ram/stm32mp1/stm32mp1_tests.h b/drivers/ram/stm32mp1/stm32mp1_tests.h
new file mode 100644
index 0000000..a69fc0a
--- /dev/null
+++ b/drivers/ram/stm32mp1/stm32mp1_tests.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef _RAM_STM32MP1_TESTS_H_
+#define _RAM_STM32MP1_TESTS_H_
+
+#include "stm32mp1_ddr_regs.h"
+
+enum test_result {
+	TEST_PASSED,
+	TEST_FAILED,
+	TEST_ERROR
+};
+
+struct test_desc {
+	enum test_result (*fct)(struct stm32mp1_ddrctl *ctl,
+				struct stm32mp1_ddrphy *phy,
+				char *string,
+				int argc, char *argv[]);
+	const char *name;
+	const char *usage;
+	const char *help;
+	u8 max_args;
+};
+
+extern const struct test_desc test[];
+extern const int test_nb;
+
+extern const struct test_desc tuning[];
+extern const int tuning_nb;
+
+#endif
diff --git a/drivers/ram/stm32mp1/stm32mp1_tuning.c b/drivers/ram/stm32mp1/stm32mp1_tuning.c
new file mode 100644
index 0000000..0778921
--- /dev/null
+++ b/drivers/ram/stm32mp1/stm32mp1_tuning.c
@@ -0,0 +1,1504 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#if defined(DEBUG)
+#define DEBUG_BIST
+#endif
+
+#include <common.h>
+#include <console.h>
+#include <clk.h>
+#include <ram.h>
+#include <reset.h>
+#include <asm/io.h>
+
+#include "stm32mp1_ddr_regs.h"
+#include "stm32mp1_ddr.h"
+#include "stm32mp1_tests.h"
+#include "stm32mp1_tuning.h"
+
+/* TODO make enum for nominal_delay_m_1.... (for all arrays) */
+
+/* 36deg, 54deg, 72deg, 90deg, 108deg, 126deg, 144deg */
+const u8 dx_dll_phase[7] = {3, 2, 1, 0, 14, 13, 12};
+
+/* New DQ delay value (index). These values are set during Deskew algo */
+u8 deskew_delay[NUM_BYTES][8];
+
+/*If there is still skew on a bit, mark this bit. */
+u8 deskew_non_converge[NUM_BYTES][8];
+
+/*Stores the DQS trim values (PHASE index, unit index) */
+u8 eye_training_val[NUM_BYTES][2];
+
+/* stores the log of pass/fail */
+u8 dqs_gating[NUM_BYTES][MAX_GSL_IDX + 1][MAX_GPS_IDX + 1];
+
+/* stores the dqs gate values (gsl index, gps index) */
+u8 dqs_gate_values[NUM_BYTES][2];
+
+static void itm_soft_reset(struct stm32mp1_ddrphy *phy);
+static u8 set_midpoint_read_dqs_gating(struct stm32mp1_ddrphy *phy, u8 byte);
+
+static u8 BIST_error_max = 1;
+static u32 BIST_seed = 0x1234ABCD;
+
+static u8 get_nb_bytes(struct stm32mp1_ddrctl *ctl)
+{
+	u32 data_bus = readl(&ctl->mstr) & DDRCTRL_MSTR_DATA_BUS_WIDTH_MASK;
+	u8 nb_bytes = NUM_BYTES;
+
+	switch (data_bus) {
+	case DDRCTRL_MSTR_DATA_BUS_WIDTH_HALF:
+		nb_bytes /= 2;
+		break;
+	case DDRCTRL_MSTR_DATA_BUS_WIDTH_QUARTER:
+		nb_bytes /= 4;
+		break;
+	default:
+		break;
+	}
+
+	return nb_bytes;
+}
+
+/* Read DQS PHASE delay  register and provides the index of the retrieved
+ * value in dx_dll_phase array.
+ */
+u8 DQS_phase_index(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u32 addr = DXNDLLCR(phy, byte);
+	u32 sdphase = 0;
+	u8 index = 0;
+
+	sdphase = (readl(addr) & DDRPHYC_DXNDLLCR_SDPHASE_MASK)
+		   >> DDRPHYC_DXNDLLCR_SDPHASE_SHIFT;
+
+	switch (sdphase) {
+	case 0x0:		/* 90deg */
+	case 0x5:
+	case 0xA:
+	case 0xF:
+		index = 3;	/* The index of 90deg in dx_dll_phase */
+		break;
+
+	case 0x2:		/* 54deg */
+	case 0x7:
+		index = 1;
+		break;
+
+	case 0x1:		/* 72deg */
+	case 0x6:
+	case 0xB:
+		index = 2;
+		break;
+
+	case 0x3:		/* 36deg */
+		index = 0;
+		break;
+
+	case 0x4:		/* 108deg */
+	case 0x9:
+	case 0xe:
+		index = 4;
+		break;
+
+	case 0x8:		/* 126deg */
+	case 0xd:
+		index = 5;
+		break;
+
+	case 0xc:		/* 144deg */
+		index = 6;
+		break;
+
+	default:
+		index = 3;	/* 90deg */
+	}
+
+	pr_debug("%s: [%x]: %x => phase = %x -> DQS phase index = %d\n",
+		 __func__, addr, readl(addr), sdphase, index);
+
+	return index;
+}
+
+/* Read DQS unit delay register and provides the index of the retrieved value
+ * We are assuming that the delay on DQS and DQSN are equal
+ */
+u8 DQS_unit_index(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u32 addr = DXNDQSTR(phy, byte);
+	u32 index;
+
+	/* We are assuming that the delay on DQS and DQSN are equal */
+	index = (readl(addr) & DDRPHYC_DXNDQSTR_DQSDLY_MASK)
+		>> DDRPHYC_DXNDQSTR_DQSDLY_SHIFT;
+
+	pr_debug("%s: [%x]: %x => DQS unit index = %x\n",
+		 __func__, addr, readl(addr), index);
+
+	return index;
+}
+
+/* Read DQ unit delay register and provides the retrieved value for DQS
+ * We are assuming that we have the same delay when clocking
+ * by DQS and when clocking by DQSN
+ */
+u8 DQ_unit_index(struct stm32mp1_ddrphy *phy, u8 byte, u8 bit)
+{
+	u32 index;
+	u32 addr = DXNDQTR(phy, byte);
+
+	/* We are assuming that we have the same delay when clocking by DQS
+	 * and when clocking by DQSN : use only the low bits
+	 */
+	index = (readl(addr) >> DDRPHYC_DXNDQTR_DQDLY_SHIFT(bit))
+		& DDRPHYC_DXNDQTR_DQDLY_LOW_MASK;
+
+	pr_debug("%s: [%x]: %x => DQ unit index = %x\n",
+		 __func__, addr, readl(addr), index);
+
+	return index;
+}
+
+/* read r0dgsl value */
+u8 get_r0dgsl_index(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u32 addr = DXNDQSTR(phy, byte);
+
+	return (readl(addr) & DDRPHYC_DXNDQSTR_R0DGSL_MASK)
+		>> DDRPHYC_DXNDQSTR_R0DGSL_SHIFT;
+}
+
+/*read r0dgsl value */
+u8 get_r0dgps_index(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u32 addr = DXNDQSTR(phy, byte);
+
+	return (readl(addr) & DDRPHYC_DXNDQSTR_R0DGPS_MASK)
+		>> DDRPHYC_DXNDQSTR_R0DGPS_SHIFT;
+}
+
+/* Sets the DQS phase delay for a byte lane.
+ *phase delay is specified by giving the index of the desired delay
+ * in the dx_dll_phase array.
+ */
+void DQS_phase_delay(struct stm32mp1_ddrphy *phy, u8 byte, u8 phase_idx)
+{
+	u8 sdphase_val = 0;
+
+	/*	Write DXNDLLCR.SDPHASE = dx_dll_phase(phase_index); */
+	sdphase_val = dx_dll_phase[phase_idx];
+	clrsetbits_le32(DXNDLLCR(phy, byte),
+			DDRPHYC_DXNDLLCR_SDPHASE_MASK,
+			sdphase_val << DDRPHYC_DXNDLLCR_SDPHASE_SHIFT);
+}
+
+/* Sets the DQS unit delay for a byte lane.
+ * unit delay is specified by giving the index of the desired delay
+ * for dgsdly and dqsndly (same value).
+ */
+void DQS_unit_delay(struct stm32mp1_ddrphy *phy,
+		    u8 byte, u8 unit_dly_idx)
+{
+	/* Write the same value in DXNDQSTR.DQSDLY and DXNDQSTR.DQSNDLY */
+	clrsetbits_le32(DXNDQSTR(phy, byte),
+			DDRPHYC_DXNDQSTR_DQSDLY_MASK |
+			DDRPHYC_DXNDQSTR_DQSNDLY_MASK,
+			(unit_dly_idx << DDRPHYC_DXNDQSTR_DQSDLY_SHIFT) |
+			(unit_dly_idx << DDRPHYC_DXNDQSTR_DQSNDLY_SHIFT));
+
+	/* After changing this value, an ITM soft reset (PIR.ITMSRST=1,
+	 * plus PIR.INIT=1) must be issued.
+	 */
+	stm32mp1_ddrphy_init(phy, DDRPHYC_PIR_ITMSRST);
+}
+
+/* Sets the DQ unit delay for a bit line in particular byte lane.
+ * unit delay is specified by giving the desired delay
+ */
+void set_DQ_unit_delay(struct stm32mp1_ddrphy *phy,
+		       u8 byte, u8 bit,
+		       u8 dq_delay_index)
+{
+	u8 dq_bit_delay_val = dq_delay_index | (dq_delay_index << 2);
+
+	/* same value on delay for clock DQ an DQS_b */
+	clrsetbits_le32(DXNDQTR(phy, byte),
+			DDRPHYC_DXNDQTR_DQDLY_MASK
+			<< DDRPHYC_DXNDQTR_DQDLY_SHIFT(bit),
+			dq_bit_delay_val << DDRPHYC_DXNDQTR_DQDLY_SHIFT(bit));
+}
+
+void set_r0dgsl_delay(struct stm32mp1_ddrphy *phy,
+		      u8 byte, u8 r0dgsl_idx)
+{
+	clrsetbits_le32(DXNDQSTR(phy, byte),
+			DDRPHYC_DXNDQSTR_R0DGSL_MASK,
+			r0dgsl_idx << DDRPHYC_DXNDQSTR_R0DGSL_SHIFT);
+}
+
+void set_r0dgps_delay(struct stm32mp1_ddrphy *phy,
+		      u8 byte, u8 r0dgps_idx)
+{
+	clrsetbits_le32(DXNDQSTR(phy, byte),
+			DDRPHYC_DXNDQSTR_R0DGPS_MASK,
+			r0dgps_idx << DDRPHYC_DXNDQSTR_R0DGPS_SHIFT);
+}
+
+/* Basic BIST configuration for data lane tests. */
+void config_BIST(struct stm32mp1_ddrphy *phy)
+{
+	/* Selects the SDRAM bank address to be used during BIST. */
+	u32 bbank = 0;
+	/* Selects the SDRAM row address to be used during BIST. */
+	u32 brow = 0;
+	/* Selects the SDRAM column address to be used during BIST. */
+	u32 bcol = 0;
+	/* Selects the value by which the SDRAM address is incremented
+	 * for each write/read access.
+	 */
+	u32 bainc = 0x00000008;
+	/* Specifies the maximum SDRAM rank to be used during BIST.
+	 * The default value is set to maximum ranks minus 1.
+	 * must be 0 with single rank
+	 */
+	u32 bmrank = 0;
+	/* Selects the SDRAM rank to be used during BIST.
+	 * must be 0 with single rank
+	 */
+	u32 brank = 0;
+	/* Specifies the maximum SDRAM bank address to be used during
+	 * BIST before the address & increments to the next rank.
+	 */
+	u32 bmbank = 1;
+	/* Specifies the maximum SDRAM row address to be used during
+	 * BIST before the address & increments to the next bank.
+	 */
+	u32 bmrow = 0x7FFF; /* To check */
+	/* Specifies the maximum SDRAM column address to be used during
+	 * BIST before the address & increments to the next row.
+	 */
+	u32 bmcol = 0x3FF;  /* To check */
+	u32 bmode_conf = 0x00000001;  /* DRam mode */
+	u32 bdxen_conf = 0x00000001;  /* BIST on Data byte */
+	u32 bdpat_conf = 0x00000002;  /* Select LFSR pattern */
+
+	/*Setup BIST for DRAM mode,  and LFSR-random data pattern.*/
+	/*Write BISTRR.BMODE = 1?b1;*/
+	/*Write BISTRR.BDXEN = 1?b1;*/
+	/*Write BISTRR.BDPAT = 2?b10;*/
+
+	/* reset BIST */
+	writel(0x3, &phy->bistrr);
+
+	writel((bmode_conf << 3) | (bdxen_conf << 14) | (bdpat_conf << 17),
+	       &phy->bistrr);
+
+	/*Setup BIST Word Count*/
+	/*Write BISTWCR.BWCNT = 16?b0008;*/
+	writel(0x00000200, &phy->bistwcr); /* A multiple of BL/2 */
+
+	writel(bcol | (brow << 12) | (bbank << 28), &phy->bistar0);
+	writel(brank | (bmrank << 2) | (bainc << 4), &phy->bistar1);
+
+	/* To check this line : */
+	writel(bmcol | (bmrow << 12) | (bmbank << 28), &phy->bistar2);
+}
+
+/* Select the Byte lane to be tested by BIST. */
+void BIST_datx8_sel(struct stm32mp1_ddrphy *phy, u8 datx8)
+{
+	clrsetbits_le32(&phy->bistrr,
+			DDRPHYC_BISTRR_BDXSEL_MASK,
+			datx8 << DDRPHYC_BISTRR_BDXSEL_SHIFT);
+
+	/*(For example, selecting Byte Lane 3, BISTRR.BDXSEL = 4?b0011)*/
+	/* Write BISTRR.BDXSEL = datx8; */
+}
+
+/* Perform BIST Write_Read test on a byte lane and return test result. */
+void BIST_test(struct stm32mp1_ddrphy *phy, u8 byte,
+	       struct BIST_result *bist)
+{
+	bool result = true; /* BIST_SUCCESS */
+	u32 cnt = 0;
+	u32 error = 0;
+
+	bist->test_result = true;
+
+run:
+	itm_soft_reset(phy);
+
+	/*Perform BIST Reset*/
+	/* Write BISTRR.BINST = 3?b011; */
+	clrsetbits_le32(&phy->bistrr,
+			0x00000007,
+			0x00000003);
+
+	/*Re-seed LFSR*/
+	/* Write BISTLSR.SEED = 32'h1234ABCD; */
+	if (BIST_seed)
+		writel(BIST_seed, &phy->bistlsr);
+	else
+		writel(rand(), &phy->bistlsr);
+
+	/* some delay to reset BIST */
+	mdelay(1);
+
+	/*Perform BIST Run*/
+	clrsetbits_le32(&phy->bistrr,
+			0x00000007,
+			0x00000001);
+	/* Write BISTRR.BINST = 3?b001; */
+
+	/* Wait for a number of CTL clocks before reading BIST register*/
+	/* Wait 300 ctl_clk cycles;  ... IS it really needed?? */
+	/* Perform BIST Instruction Stop*/
+	/* Write BISTRR.BINST = 3?b010;*/
+
+	/* poll on BISTGSR.BDONE. If 0, wait.  ++TODO Add timeout */
+	while (!(readl(&phy->bistgsr) & DDRPHYC_BISTGSR_BDDONE))
+		;
+
+	/*Check if received correct number of words*/
+	/* if (Read BISTWCSR.DXWCNT = Read BISTWCR.BWCNT) */
+	if (((readl(&phy->bistwcsr)) >> DDRPHYC_BISTWCSR_DXWCNT_SHIFT) ==
+	    readl(&phy->bistwcr)) {
+		/*Determine if there is a data comparison error*/
+		/* if (Read BISTGSR.BDXERR = 1?b0) */
+		if (readl(&phy->bistgsr) & DDRPHYC_BISTGSR_BDXERR)
+			result = false; /* BIST_FAIL; */
+		else
+			result = true; /* BIST_SUCCESS; */
+	} else {
+		result = false; /* BIST_FAIL; */
+	}
+
+	/* loop while success */
+	cnt++;
+	if (result && cnt != 1000)
+		goto run;
+
+	if (!result) {
+		error++;
+#ifdef DEBUG_BIST
+		if (error < 5)
+			printf("     %d: %d, bistwcsr=0x%x, bistgsr=0x%x\n",
+			       error, cnt, readl(&phy->bistwcsr),
+			       readl(&phy->bistgsr));
+#endif
+	}
+	if (error < BIST_error_max) {
+		if (cnt != 1000)
+			goto run;
+		bist->test_result = true;
+	} else {
+		bist->test_result = false;
+	}
+#ifdef DEBUG_BIST
+	if (cnt != 1000)
+		goto run;
+	printf("  - BIST result = %d, error=%d\n", bist->test_result, error);
+#endif
+}
+
+/* Init the Write_Read result struct. */
+void init_result_struct(struct BIST_result *result)
+{
+	u8 i = 0;
+
+	/* Init with an overall "true" (success) condition.
+	 * Any fail at any bit will set this to false
+	 */
+	result->test_result = true;
+	result->all_bits_fail = false;
+
+	/* Init with an  "true" (success) condition for bit_i.
+	 * A fail at this bit will set this to false
+	 */
+	for (i = 0; i < 8; i++)
+		result->bit_i_test_result[i] = true;
+}
+
+/* After running the deskew algo, this function applies the new DQ delays
+ * by reading them from the array "deskew_delay"and writing in PHY registers.
+ * The bits that are not deskewed parfectly (too much skew on them,
+ * or data eye very wide) are marked in the array deskew_non_converge.
+ */
+void apply_deskew_results(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u8  bit_i;
+	u8  index;
+
+	for (bit_i = 0; bit_i < 8; bit_i++) {
+		set_DQ_unit_delay(phy, byte, bit_i, deskew_delay[byte][bit_i]);
+		index = DQ_unit_index(phy, byte, bit_i);
+		pr_debug("Byte %d ; bit %d : The new DQ delay (%d) index=%d [delta=%d, 3 is the default]",
+			 byte, bit_i, deskew_delay[byte][bit_i],
+			 index, index - 3);
+		printf("Byte %d, bit %d, DQ delay = %d",
+		       byte, bit_i, deskew_delay[byte][bit_i]);
+		if (deskew_non_converge[byte][bit_i] == 1)
+			pr_debug(" - not converged : still more skew");
+		printf("\n");
+	}
+}
+
+/* DQ Bit de-skew algorithm.
+ * Deskews data lines as much as possible.
+ * 1. Add delay to DQS line until finding the failure
+ *    (normally a hold time violation)
+ * 2. Reduce DQS line by small steps until finding the very first time
+ *    we go back to "Pass" condition.
+ * 3. For each DQ line, Reduce DQ delay until finding the very first failure
+ *    (normally a hold time fail)
+ * 4. When all bits are at their first failure delay, we can consider them
+ *    aligned.
+ * Handle conrer situation (Can't find Pass-fail, or fail-pass transitions
+ * at any step)
+ * TODO Provide a return Status. Improve doc
+ */
+static enum test_result bit_deskew(struct stm32mp1_ddrctl *ctl,
+				   struct stm32mp1_ddrphy *phy, char *string)
+{
+	struct BIST_result result;
+	s8 dqs_unit_delay_index = 0;
+	u8 datx8 = 0;
+	u8 bit_i = 0;
+	s8 phase_idx = 0;
+	s8 bit_i_delay_index = 0;
+	u8 success = 0;
+	struct tuning_position last_right_ok;
+	u8 force_stop = 0;
+	u8 fail_found;
+	u8 error = 0;
+	u8 nb_bytes = get_nb_bytes(ctl);
+	/* u8 last_pass_dqs_unit = 0; */
+
+	memset(deskew_delay, 0, sizeof(deskew_delay));
+	memset(deskew_non_converge, 0, sizeof(deskew_non_converge));
+
+	/*Disable DQS Drift Compensation*/
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_DFTCMP);
+	/*Disable all bytes*/
+	/* Disable automatic power down of DLL and IOs when disabling
+	 * a byte (To avoid having to add programming and  delay
+	 * for a DLL re-lock when later re-enabling a disabled Byte Lane)
+	 */
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_PDDISDX);
+
+	/* Disable all data bytes */
+	clrbits_le32(&phy->dx0gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx1gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx2gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx3gcr, DDRPHYC_DXNGCR_DXEN);
+
+	/* Config the BIST block */
+	config_BIST(phy);
+	pr_debug("BIST Config done.\n");
+
+	/* Train each byte */
+	for (datx8 = 0; datx8 < nb_bytes; datx8++) {
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				datx8 + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		pr_debug("\n======================\n");
+		pr_debug("Start deskew byte %d .\n", datx8);
+		pr_debug("======================\n");
+		/* Enable Byte (DXNGCR, bit DXEN) */
+		setbits_le32(DXNGCR(phy, datx8), DDRPHYC_DXNGCR_DXEN);
+
+		/* Select the byte lane for comparison of read data */
+		BIST_datx8_sel(phy, datx8);
+
+		/* Set all DQDLYn to maximum value. All bits within the byte
+		 * will be delayed with DQSTR = 2 instead of max = 3
+		 * to avoid inter bits fail influence
+		 */
+		writel(0xAAAAAAAA, DXNDQTR(phy, datx8));
+
+		/* Set the DQS phase delay to 90 DEG (default).
+		 * What is defined here is the index of the desired config
+		 * in the PHASE array.
+		 */
+		phase_idx = _90deg;
+
+		/* Set DQS unit delay to the max value. */
+		dqs_unit_delay_index = MAX_DQS_UNIT_IDX;
+		DQS_unit_delay(phy, datx8, dqs_unit_delay_index);
+		DQS_phase_delay(phy, datx8, phase_idx);
+
+		/* Issue a DLL soft reset */
+		clrbits_le32(DXNDLLCR(phy, datx8), DDRPHYC_DXNDLLCR_DLLSRST);
+		setbits_le32(DXNDLLCR(phy, datx8), DDRPHYC_DXNDLLCR_DLLSRST);
+
+		/* Test this typical init condition */
+		BIST_test(phy, datx8, &result);
+		success = result.test_result;
+
+		/* If the test pass in this typical condition,
+		 * start the algo with it.
+		 * Else, look for Pass init condition
+		 */
+		if (!success) {
+			pr_debug("Fail at init condtion. Let's look for a good init condition.\n");
+			success = 0; /* init */
+			/* Make sure we start with a PASS condition before
+			 * looking for a fail condition.
+			 * Find the first PASS PHASE condition
+			 */
+
+			/* escape if we find a PASS */
+			pr_debug("increase Phase idx\n");
+			while (!success && (phase_idx <= MAX_DQS_PHASE_IDX)) {
+				DQS_phase_delay(phy, datx8, phase_idx);
+				BIST_test(phy, datx8, &result);
+				success = result.test_result;
+				phase_idx++;
+			}
+			/* if ended with success
+			 * ==>> Restore the fist success condition
+			 */
+			if (success)
+				phase_idx--; /* because it ended with ++ */
+		}
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				datx8 + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		/* We couldn't find a successful condition, its seems
+		 * we have hold violation, lets try reduce DQS_unit Delay
+		 */
+		if (!success) {
+			/* We couldn't find a successful condition, its seems
+			 * we have hold violation, lets try reduce DQS_unit
+			 * Delay
+			 */
+			pr_debug("Still fail. Try decrease DQS Unit delay\n");
+
+			phase_idx = 0;
+			dqs_unit_delay_index = 0;
+			DQS_phase_delay(phy, datx8, phase_idx);
+
+			/* escape if we find a PASS */
+			while (!success &&
+			       (dqs_unit_delay_index <=
+				MAX_DQS_UNIT_IDX)) {
+				DQS_unit_delay(phy, datx8,
+					       dqs_unit_delay_index);
+				BIST_test(phy, datx8, &result);
+				success = result.test_result;
+				dqs_unit_delay_index++;
+			}
+			if (success) {
+				/* Restore the first success condition*/
+				dqs_unit_delay_index--;
+				/* last_pass_dqs_unit = dqs_unit_delay_index;*/
+				DQS_unit_delay(phy, datx8,
+					       dqs_unit_delay_index);
+			} else {
+				/* No need to continue,
+				 * there is no pass region.
+				 */
+				force_stop = 1;
+			}
+		}
+
+		/* There is an initial PASS condition
+		 * Look for the first failing condition by PHASE stepping.
+		 * This part of the algo can finish without converging.
+		 */
+		if (force_stop) {
+			printf("Result: Failed ");
+			printf("[Cannot Deskew lines, ");
+			printf("there is no PASS region]\n");
+			error++;
+			continue;
+		}
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				datx8 + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+
+		pr_debug("there is a pass region for phase idx %d\n",
+			 phase_idx);
+		pr_debug("Step1: Find the first failing condition\n");
+		/* Look for the first failing condition by PHASE stepping.
+		 * This part of the algo can finish without converging.
+		 */
+
+		/* escape if we find a fail (hold time violation)
+		 * condition at any bit or if out of delay range.
+		 */
+		while (success && (phase_idx <= MAX_DQS_PHASE_IDX)) {
+			DQS_phase_delay(phy, datx8, phase_idx);
+			BIST_test(phy, datx8, &result);
+			success = result.test_result;
+			phase_idx++;
+		}
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				datx8 + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+
+		/* if the loop ended with a failing condition at any bit,
+		 * lets look for the first previous success condition by unit
+		 * stepping (minimal delay)
+		 */
+		if (!success) {
+			pr_debug("Fail region (PHASE) found phase idx %d\n",
+				 phase_idx);
+			pr_debug("Let's look for first success by DQS Unit steps\n");
+			/* This part, the algo always converge */
+			phase_idx--;
+
+			/* escape if we find a success condition
+			 * or if out of delay range.
+			 */
+			while (!success && dqs_unit_delay_index >= 0) {
+				DQS_unit_delay(phy, datx8,
+					       dqs_unit_delay_index);
+				BIST_test(phy, datx8, &result);
+				success = result.test_result;
+				dqs_unit_delay_index--;
+			}
+			/* if the loop ended with a success condition,
+			 * the last delay Right OK (before hold violation)
+			 *  condition is then defined as following:
+			 */
+			if (success) {
+				/* Hold the dely parameters of the the last
+				 * delay Right OK condition.
+				 * -1 to get back to current condition
+				 */
+				last_right_ok.phase = phase_idx;
+				/*+1 to get back to current condition */
+				last_right_ok.unit = dqs_unit_delay_index + 1;
+				last_right_ok.bits_delay = 0xFFFFFFFF;
+				pr_debug("Found %d\n", dqs_unit_delay_index);
+			} else {
+				/* the last OK condition is then with the
+				 * previous phase_idx.
+				 * -2 instead of -1 because at the last
+				 * iteration of the while(),
+				 * we incremented phase_idx
+				 */
+				last_right_ok.phase = phase_idx - 1;
+				/* Nominal+1. Because we want the previous
+				 * delay after reducing the phase delay.
+				 */
+				last_right_ok.unit = 1;
+				last_right_ok.bits_delay = 0xFFFFFFFF;
+				pr_debug("Not Found : try previous phase %d\n",
+					 phase_idx - 1);
+
+				DQS_phase_delay(phy, datx8, phase_idx - 1);
+				dqs_unit_delay_index = 0;
+				success = true;
+				while (success &&
+				       (dqs_unit_delay_index <
+					MAX_DQS_UNIT_IDX)) {
+					DQS_unit_delay(phy, datx8,
+						       dqs_unit_delay_index);
+					BIST_test(phy, datx8, &result);
+					success = result.test_result;
+					dqs_unit_delay_index++;
+					pr_debug("dqs_unit_delay_index = %d, result = %d\n",
+						 dqs_unit_delay_index, success);
+				}
+
+				if (!success) {
+					last_right_ok.unit =
+						 dqs_unit_delay_index - 1;
+				} else {
+					last_right_ok.unit = 0;
+					pr_debug("ERROR: failed region not FOUND");
+				}
+			}
+		} else {
+			/* we can't find a failing  condition at all bits
+			 * ==> Just hold the last test condition
+			 * (the max DQS delay)
+			 * which is the most likely,
+			 * the closest to a hold violation
+			 * If we can't find a Fail condition after
+			 * the Pass region, stick at this position
+			 * In order to have max chances to find a fail
+			 * when reducing DQ delays.
+			 */
+			last_right_ok.phase = MAX_DQS_PHASE_IDX;
+			last_right_ok.unit = MAX_DQS_UNIT_IDX;
+			last_right_ok.bits_delay = 0xFFFFFFFF;
+			pr_debug("Can't find the a fail condition\n");
+		}
+
+		/* step 2:
+		 * if we arrive at this stage, it means that we found the last
+		 * Right OK condition (by tweeking the DQS delay). Or we simply
+		 * pushed DQS delay to the max
+		 * This means that by reducing the delay on some DQ bits,
+		 * we should find a failing condition.
+		 */
+		printf("Byte %d, DQS unit = %d, phase = %d\n",
+		       datx8, last_right_ok.unit, last_right_ok.phase);
+		pr_debug("Step2, unit = %d, phase = %d, bits delay=%x\n",
+			 last_right_ok.unit, last_right_ok.phase,
+			 last_right_ok.bits_delay);
+
+		/* Restore the last_right_ok condtion. */
+		DQS_unit_delay(phy, datx8, last_right_ok.unit);
+		DQS_phase_delay(phy, datx8, last_right_ok.phase);
+		writel(last_right_ok.bits_delay, DXNDQTR(phy, datx8));
+
+		/* train each bit
+		 * reduce delay on each bit, and perform a write/read test
+		 * and stop at the very first time it fails.
+		 * the goal is the find the first failing condition
+		 * for each bit.
+		 * When we achieve this condition<  for all the bits,
+		 * we are sure they are aligned (+/- step resolution)
+		 */
+		fail_found = 0;
+		for (bit_i = 0; bit_i < 8; bit_i++) {
+			if (ctrlc()) {
+				sprintf(string,
+					"interrupted at byte %d/%d, error=%d",
+					datx8 + 1, nb_bytes, error);
+				return error;
+			}
+#ifdef DEBUG_BIST
+			printf("deskewing bit %d:\n", bit_i);
+#else
+			pr_debug("deskewing bit %d:\n", bit_i);
+#endif
+			success = 1; /* init */
+			/* Set all DQDLYn to maximum value.
+			 * Only bit_i will be down-delayed
+			 * ==> if we have a fail, it will be definitely
+			 *     from bit_i
+			 */
+			writel(0xFFFFFFFF, DXNDQTR(phy, datx8));
+			/* Arriving at this stage,
+			 * we have a success condition with delay = 3;
+			 */
+			bit_i_delay_index = 3;
+
+			/* escape if bit delay is out of range or
+			 * if a fatil occurs
+			 */
+			while ((bit_i_delay_index >= 0) && success) {
+				set_DQ_unit_delay(phy, datx8,
+						  bit_i,
+						  bit_i_delay_index);
+#ifdef DEBUG_BIST
+				printf(" delay_index %d:\n", bit_i_delay_index);
+#endif
+				BIST_test(phy, datx8, &result);
+				success = result.test_result;
+				bit_i_delay_index--;
+			}
+#ifdef DEBUG_BIST
+			{	/* check next error */
+				int delay_index = bit_i_delay_index;
+
+				while (delay_index >= 0) {
+					set_DQ_unit_delay(phy, datx8, bit_i,
+							  delay_index);
+					printf(" delay_index %d:\n",
+					       delay_index);
+					BIST_test(phy, datx8, &result);
+					delay_index--;
+				}
+			}
+#endif
+
+			/* if escape with a fail condition
+			 * ==> save this position for bit_i
+			 */
+			if (!success) {
+				/* save the delay position.
+				 * Add 1 because the while loop ended with a --,
+				 * and that we need to hold the last success
+				 *  delay
+				 */
+				deskew_delay[datx8][bit_i] =
+					bit_i_delay_index + 2;
+				if (deskew_delay[datx8][bit_i] > 3)
+					deskew_delay[datx8][bit_i] = 3;
+
+				/* A flag that states we found at least a fail
+				 * at one bit.
+				 */
+				fail_found = 1;
+				pr_debug("Fail found on bit %d, for delay = %d => deskew[%d][%d] = %d\n",
+					 bit_i, bit_i_delay_index + 1,
+					 datx8, bit_i,
+					 deskew_delay[datx8][bit_i]);
+			} else {
+				/* if we can find a success condition by
+				 * back-delaying this bit, just set the delay
+				 * to 0 (the best deskew
+				 * possible) and mark the bit.
+				 */
+				deskew_delay[datx8][bit_i] = 0;
+				/* set a flag that will be used later
+				 * in the report.
+				 */
+				deskew_non_converge[datx8][bit_i] = 1;
+				pr_debug("Fail not found on bit %d => deskew[%d][%d] = %d\n",
+					 bit_i, datx8, bit_i,
+					 deskew_delay[datx8][bit_i]);
+			}
+		}
+		pr_debug("**********byte %d tuning complete************\n",
+			 datx8);
+		/* If we can't find any failure by back delaying DQ lines,
+		 * hold the default values
+		 */
+		if (!fail_found) {
+			for (bit_i = 0; bit_i < 8; bit_i++)
+				deskew_delay[datx8][bit_i] = 0;
+			pr_debug("The Deskew algorithm can't converge, there is too much margin in your design. Good job!\n");
+		}
+
+		apply_deskew_results(phy, datx8);
+		/* Restore nominal value for DQS delay */
+		DQS_phase_delay(phy, datx8, 3);
+		DQS_unit_delay(phy, datx8, 3);
+		/* disable byte after byte bits deskew */
+		clrbits_le32(DXNGCR(phy, datx8), DDRPHYC_DXNGCR_DXEN);
+	}  /* end of byte deskew */
+
+	/* re-enable all data bytes */
+	setbits_le32(&phy->dx0gcr, DDRPHYC_DXNGCR_DXEN);
+	setbits_le32(&phy->dx1gcr, DDRPHYC_DXNGCR_DXEN);
+	setbits_le32(&phy->dx2gcr, DDRPHYC_DXNGCR_DXEN);
+	setbits_le32(&phy->dx3gcr, DDRPHYC_DXNGCR_DXEN);
+
+	if (error) {
+		sprintf(string, "error = %d", error);
+		return TEST_FAILED;
+	}
+
+	return TEST_PASSED;
+} /* end function */
+
+/* TODO: Check if how delay blocks work. Maybe I have a wrong assumption!!
+ * Can check with Tuning App note.
+ */
+
+/* Trim DQS timings and set it in the centre of data eye.
+ * Look for a PPPPF region, then look for a FPPP region and finally select
+ * the mid of the FPPPPPF region
+ */
+
+static enum test_result eye_training(struct stm32mp1_ddrctl *ctl,
+				     struct stm32mp1_ddrphy *phy, char *string)
+{
+	u8 byte = 0;
+	struct BIST_result result;
+	s8 dqs_unit_delay_index = 0;
+	s8 phase_idx = 0;
+	s8 dqs_unit_delay_index_pass = 0;
+	s8 phase_idx_pass = 0;
+	u8 success = 0;
+	u8 left_phase_bound_found, right_phase_bound_found;
+	u8 left_unit_bound_found, right_unit_bound_found;
+	u8 left_bound_found, right_bound_found;
+	struct tuning_position left_bound, right_bound;
+	u8 error = 0;
+	u8 nb_bytes = get_nb_bytes(ctl);
+
+	/*Disable DQS Drift Compensation*/
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_DFTCMP);
+	/*Disable all bytes*/
+	/* Disable automatic power down of DLL and IOs when disabling a byte
+	 * (To avoid having to add programming and  delay
+	 * for a DLL re-lock when later re-enabling a disabled Byte Lane)
+	 */
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_PDDISDX);
+
+	/*Disable all data bytes */
+	clrbits_le32(&phy->dx0gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx1gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx2gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx3gcr, DDRPHYC_DXNGCR_DXEN);
+
+	/* Config the BIST block */
+	config_BIST(phy);
+
+	for (byte = 0; byte < nb_bytes; byte++) {
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				byte + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		right_bound.phase = 0;
+		right_bound.unit = 0;
+
+		left_bound.phase = 0;
+		left_bound.unit = 0;
+
+		left_phase_bound_found = 0;
+		right_phase_bound_found = 0;
+
+		left_unit_bound_found = 0;
+		right_unit_bound_found = 0;
+
+		left_bound_found = 0;
+		right_bound_found = 0;
+
+		/* Enable Byte (DXNGCR, bit DXEN) */
+		setbits_le32(DXNGCR(phy, byte), DDRPHYC_DXNGCR_DXEN);
+
+		/* Select the byte lane for comparison of read data */
+		BIST_datx8_sel(phy, byte);
+
+		/* Set DQS phase delay to the nominal value. */
+		phase_idx = _90deg;
+		phase_idx_pass = phase_idx;
+
+		/* Set DQS unit delay to the nominal value. */
+		dqs_unit_delay_index = 3;
+		dqs_unit_delay_index_pass = dqs_unit_delay_index;
+		success = 0;
+
+		pr_debug("STEP0: Find Init delay\n");
+		/* STEP0: Find Init delay: a delay that put the system
+		 * in a "Pass" condition then (TODO) update
+		 * dqs_unit_delay_index_pass & phase_idx_pass
+		 */
+		DQS_unit_delay(phy, byte, dqs_unit_delay_index);
+		DQS_phase_delay(phy, byte, phase_idx);
+		BIST_test(phy, byte, &result);
+		success = result.test_result;
+		/* If we have a fail in the nominal condition */
+		if (!success) {
+			/* Look at the left */
+			while (phase_idx >= 0 && !success) {
+				phase_idx--;
+				DQS_phase_delay(phy, byte, phase_idx);
+				BIST_test(phy, byte, &result);
+				success = result.test_result;
+			}
+		}
+		if (!success) {
+			/* if we can't find pass condition,
+			 * then look at the right
+			 */
+			phase_idx = _90deg;
+			while (phase_idx <= MAX_DQS_PHASE_IDX &&
+			       !success) {
+				phase_idx++;
+				DQS_phase_delay(phy, byte,
+						phase_idx);
+				BIST_test(phy, byte, &result);
+				success = result.test_result;
+			}
+		}
+		/* save the pass condition */
+		if (success) {
+			phase_idx_pass = phase_idx;
+		} else {
+			printf("Result: Failed ");
+			printf("[Cannot DQS timings, ");
+			printf("there is no PASS region]\n");
+			error++;
+			continue;
+		}
+
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				byte + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		pr_debug("STEP1: Find LEFT PHASE DQS Bound\n");
+		/* STEP1: Find LEFT PHASE DQS Bound */
+		while ((phase_idx >= 0) &&
+		       (phase_idx <= MAX_DQS_PHASE_IDX) &&
+		       !left_phase_bound_found) {
+			DQS_unit_delay(phy, byte,
+				       dqs_unit_delay_index);
+			DQS_phase_delay(phy, byte,
+					phase_idx);
+			BIST_test(phy, byte, &result);
+			success = result.test_result;
+
+			/*TODO: Manage the case were at the beginning
+			 * there is already a fail
+			 */
+			if (!success) {
+				/* the last pass condition */
+				left_bound.phase = ++phase_idx;
+				left_phase_bound_found = 1;
+			} else if (success) {
+				phase_idx--;
+			}
+		}
+		if (!left_phase_bound_found) {
+			left_bound.phase = 0;
+			phase_idx = 0;
+		}
+		/* If not found, lets take 0 */
+
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				byte + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		pr_debug("STEP2: Find UNIT left bound\n");
+		/* STEP2: Find UNIT left bound */
+		while ((dqs_unit_delay_index >= 0) &&
+		       !left_unit_bound_found) {
+			DQS_unit_delay(phy, byte,
+				       dqs_unit_delay_index);
+			DQS_phase_delay(phy, byte, phase_idx);
+			BIST_test(phy, byte, &result);
+			success = result.test_result;
+			if (!success) {
+				left_bound.unit =
+					++dqs_unit_delay_index;
+				left_unit_bound_found = 1;
+				left_bound_found = 1;
+			} else if (success) {
+				dqs_unit_delay_index--;
+			}
+		}
+
+		/* If not found, lets take 0 */
+		if (!left_unit_bound_found)
+			left_bound.unit = 0;
+
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				byte + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		pr_debug("STEP3: Find PHase right bound\n");
+		/* STEP3: Find PHase right bound, start with "pass"
+		 * condition
+		 */
+
+		/* Set DQS phase delay to the pass value. */
+		phase_idx = phase_idx_pass;
+
+		/* Set DQS unit delay to the pass value. */
+		dqs_unit_delay_index = dqs_unit_delay_index_pass;
+
+		while ((phase_idx <= MAX_DQS_PHASE_IDX) &&
+		       !right_phase_bound_found) {
+			DQS_unit_delay(phy, byte,
+				       dqs_unit_delay_index);
+			DQS_phase_delay(phy, byte, phase_idx);
+			BIST_test(phy, byte, &result);
+			success = result.test_result;
+			if (!success) {
+				/* the last pass condition */
+				right_bound.phase = --phase_idx;
+				right_phase_bound_found = 1;
+			} else if (success) {
+				phase_idx++;
+			}
+		}
+
+		/* If not found, lets take the max value */
+		if (!right_phase_bound_found) {
+			right_bound.phase = MAX_DQS_PHASE_IDX;
+			phase_idx = MAX_DQS_PHASE_IDX;
+		}
+
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d, error=%d",
+				byte + 1, nb_bytes, error);
+			return TEST_FAILED;
+		}
+		pr_debug("STEP4: Find UNIT right bound\n");
+		/* STEP4: Find UNIT right bound */
+		while ((dqs_unit_delay_index <= MAX_DQS_UNIT_IDX) &&
+		       !right_unit_bound_found) {
+			DQS_unit_delay(phy, byte,
+				       dqs_unit_delay_index);
+			DQS_phase_delay(phy, byte, phase_idx);
+			BIST_test(phy, byte, &result);
+			success = result.test_result;
+			if (!success) {
+				right_bound.unit =
+					--dqs_unit_delay_index;
+				right_unit_bound_found = 1;
+				right_bound_found = 1;
+			} else if (success) {
+				dqs_unit_delay_index++;
+			}
+		}
+		/* If not found, lets take the max value */
+		if (!right_unit_bound_found)
+			right_bound.unit = MAX_DQS_UNIT_IDX;
+
+		/* If we found a regular FAil Pass FAil pattern
+		 * FFPPPPPPFF
+		 * OR PPPPPFF  Or FFPPPPP
+		 */
+
+		if (left_bound_found || right_bound_found) {
+			eye_training_val[byte][0] = (right_bound.phase +
+						 left_bound.phase) / 2;
+			eye_training_val[byte][1] = (right_bound.unit +
+						 left_bound.unit) / 2;
+
+			/* If we already lost 1/2PHASE Tuning,
+			 * let's try to recover by ++ on unit
+			 */
+			if (((right_bound.phase + left_bound.phase) % 2 == 1) &&
+			    eye_training_val[byte][1] != MAX_DQS_UNIT_IDX)
+				eye_training_val[byte][1]++;
+			pr_debug("** found phase : %d -  %d & unit %d - %d\n",
+				 right_bound.phase, left_bound.phase,
+				 right_bound.unit, left_bound.unit);
+			pr_debug("** calculating mid region: phase: %d  unit: %d (nominal is 3)\n",
+				 eye_training_val[byte][0],
+				 eye_training_val[byte][1]);
+		} else {
+			/* PPPPPPPPPP, we're already good.
+			 * Set nominal values.
+			 */
+			eye_training_val[byte][0] = 3;
+			eye_training_val[byte][1] = 3;
+		}
+		DQS_phase_delay(phy, byte, eye_training_val[byte][0]);
+		DQS_unit_delay(phy, byte, eye_training_val[byte][1]);
+
+		printf("Byte %d, DQS unit = %d, phase = %d\n",
+		       byte,
+		       eye_training_val[byte][1],
+		       eye_training_val[byte][0]);
+	}
+
+	if (error) {
+		sprintf(string, "error = %d", error);
+		return TEST_FAILED;
+	}
+
+	return TEST_PASSED;
+}
+
+void display_reg_results(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u8 i = 0;
+
+	printf("Byte %d Dekew result, bit0 delay, bit1 delay...bit8 delay\n  ",
+	       byte);
+
+	for (i = 0; i < 8; i++)
+		printf("%d ", DQ_unit_index(phy, byte, i));
+	printf("\n");
+
+	printf("dxndllcr: [%08x] val:%08x\n",
+	       DXNDLLCR(phy, byte),
+	       readl(DXNDLLCR(phy, byte)));
+	printf("dxnqdstr: [%08x] val:%08x\n",
+	       DXNDQSTR(phy, byte),
+	       readl(DXNDQSTR(phy, byte)));
+	printf("dxndqtr: [%08x] val:%08x\n",
+	       DXNDQTR(phy, byte),
+	       readl(DXNDQTR(phy, byte)));
+}
+
+static enum test_result read_dqs_gating(struct stm32mp1_ddrctl *ctl,
+					struct stm32mp1_ddrphy *phy,
+					char *string)
+{
+	u8 byte, gsl_idx, gps_idx = 0;
+	struct BIST_result result;
+	u8 success = 0;
+	u8 nb_bytes = get_nb_bytes(ctl);
+
+	memset(dqs_gating, 0x0, sizeof(dqs_gating));
+
+	/*disable dqs drift compensation*/
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_DFTCMP);
+	/*disable all bytes*/
+	/* disable automatic power down of dll and ios when disabling a byte
+	 * (to avoid having to add programming and  delay
+	 * for a dll re-lock when later re-enabling a disabled byte lane)
+	 */
+	clrbits_le32(&phy->pgcr, DDRPHYC_PGCR_PDDISDX);
+
+	/* disable all data bytes */
+	clrbits_le32(&phy->dx0gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx1gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx2gcr, DDRPHYC_DXNGCR_DXEN);
+	clrbits_le32(&phy->dx3gcr, DDRPHYC_DXNGCR_DXEN);
+
+	/* config the bist block */
+	config_BIST(phy);
+
+	for (byte = 0; byte < nb_bytes; byte++) {
+		if (ctrlc()) {
+			sprintf(string, "interrupted at byte %d/%d",
+				byte + 1, nb_bytes);
+			return TEST_FAILED;
+		}
+		/* enable byte x (dxngcr, bit dxen) */
+		setbits_le32(DXNGCR(phy, byte), DDRPHYC_DXNGCR_DXEN);
+
+		/* select the byte lane for comparison of read data */
+		BIST_datx8_sel(phy, byte);
+		for (gsl_idx = 0; gsl_idx <= MAX_GSL_IDX; gsl_idx++) {
+			for (gps_idx = 0; gps_idx <= MAX_GPS_IDX; gps_idx++) {
+				if (ctrlc()) {
+					sprintf(string,
+						"interrupted at byte %d/%d",
+						byte + 1, nb_bytes);
+					return TEST_FAILED;
+				}
+				/* write cfg to dxndqstr */
+				set_r0dgsl_delay(phy, byte, gsl_idx);
+				set_r0dgps_delay(phy, byte, gps_idx);
+
+				BIST_test(phy, byte, &result);
+				success = result.test_result;
+				if (success)
+					dqs_gating[byte][gsl_idx][gps_idx] = 1;
+				itm_soft_reset(phy);
+			}
+		}
+		set_midpoint_read_dqs_gating(phy, byte);
+		/* dummy reads */
+		readl(0xc0000000);
+		readl(0xc0000000);
+	}
+
+	/* re-enable drift compensation */
+	/* setbits_le32(&phy->pgcr, DDRPHYC_PGCR_DFTCMP); */
+	return TEST_PASSED;
+}
+
+/* analyse the dgs gating log table, and determine the midpoint.*/
+static u8 set_midpoint_read_dqs_gating(struct stm32mp1_ddrphy *phy, u8 byte)
+{
+	u8 gsl_idx, gps_idx = 0;
+	u8 left_bound_idx[2] = {0, 0};
+	u8 right_bound_idx[2] = {0, 0};
+	u8 left_bound_found = 0;
+	u8 right_bound_found = 0;
+	u8 intermittent = 0;
+	u8 value;
+
+	for (gsl_idx = 0; gsl_idx <= MAX_GSL_IDX; gsl_idx++) {
+		for (gps_idx = 0; gps_idx <= MAX_GPS_IDX; gps_idx++) {
+			value = dqs_gating[byte][gsl_idx][gps_idx];
+			if (value == 1 && left_bound_found == 0) {
+				left_bound_idx[0] = gsl_idx;
+				left_bound_idx[1] = gps_idx;
+				left_bound_found = 1;
+			} else if (value == 0 &&
+				   left_bound_found == 1 &&
+				   !right_bound_found) {
+				if (gps_idx == 0) {
+					right_bound_idx[0] = gsl_idx - 1;
+					right_bound_idx[1] = MAX_GPS_IDX;
+				} else {
+					right_bound_idx[0] = gsl_idx;
+					right_bound_idx[1] = gps_idx - 1;
+				}
+				right_bound_found = 1;
+			} else if (value == 1 &&
+				   right_bound_found == 1) {
+				intermittent = 1;
+			}
+		}
+	}
+
+	/* if only ppppppp is found, there is no mid region. */
+	if (left_bound_idx[0] == 0 && left_bound_idx[1] == 0 &&
+	    right_bound_idx[0] == 0 && right_bound_idx[1] == 0)
+		intermittent = 1;
+
+	/*if we found a regular fail pass fail pattern ffppppppff
+	 * or pppppff  or ffppppp
+	 */
+	if (!intermittent) {
+		/*if we found a regular fail pass fail pattern ffppppppff
+		 * or pppppff  or ffppppp
+		 */
+		if (left_bound_found || right_bound_found) {
+			pr_debug("idx0(%d): %d %d      idx1(%d) : %d %d\n",
+				 left_bound_found,
+				 right_bound_idx[0], left_bound_idx[0],
+				 right_bound_found,
+				 right_bound_idx[1], left_bound_idx[1]);
+			dqs_gate_values[byte][0] =
+				(right_bound_idx[0] + left_bound_idx[0]) / 2;
+			dqs_gate_values[byte][1] =
+				(right_bound_idx[1] + left_bound_idx[1]) / 2;
+			/* if we already lost 1/2gsl tuning,
+			 * let's try to recover by ++ on gps
+			 */
+			if (((right_bound_idx[0] +
+			      left_bound_idx[0]) % 2 == 1) &&
+			    dqs_gate_values[byte][1] != MAX_GPS_IDX)
+				dqs_gate_values[byte][1]++;
+			/* if we already lost 1/2gsl tuning and gps is on max*/
+			else if (((right_bound_idx[0] +
+				   left_bound_idx[0]) % 2 == 1) &&
+				 dqs_gate_values[byte][1] == MAX_GPS_IDX) {
+				dqs_gate_values[byte][1] = 0;
+				dqs_gate_values[byte][0]++;
+			}
+			/* if we have gsl left and write limit too close
+			 * (difference=1)
+			 */
+			if (((right_bound_idx[0] - left_bound_idx[0]) == 1)) {
+				dqs_gate_values[byte][1] = (left_bound_idx[1] +
+							    right_bound_idx[1] +
+							    4) / 2;
+				if (dqs_gate_values[byte][1] >= 4) {
+					dqs_gate_values[byte][0] =
+						right_bound_idx[0];
+					dqs_gate_values[byte][1] -= 4;
+				} else {
+					dqs_gate_values[byte][0] =
+						left_bound_idx[0];
+				}
+			}
+			pr_debug("*******calculating mid region: system latency: %d  phase: %d********\n",
+				 dqs_gate_values[byte][0],
+				 dqs_gate_values[byte][1]);
+			pr_debug("*******the nominal values were system latency: 0  phase: 2*******\n");
+			set_r0dgsl_delay(phy, byte, dqs_gate_values[byte][0]);
+			set_r0dgps_delay(phy, byte, dqs_gate_values[byte][1]);
+		}
+	} else {
+		/* if intermitant, restore defaut values */
+		pr_debug("dqs gating:no regular fail/pass/fail found. defaults values restored.\n");
+		set_r0dgsl_delay(phy, byte, 0);
+		set_r0dgps_delay(phy, byte, 2);
+	}
+
+	/* return 0 if intermittent or if both left_bound
+	 * and right_bound are not found
+	 */
+	return !(intermittent || (left_bound_found && right_bound_found));
+}
+
+static void itm_soft_reset(struct stm32mp1_ddrphy *phy)
+{
+	stm32mp1_ddrphy_init(phy, DDRPHYC_PIR_ITMSRST);
+}
+
+/****************************************************************
+ * TEST
+ ****************************************************************
+ */
+static enum test_result do_read_dqs_gating(struct stm32mp1_ddrctl *ctl,
+					   struct stm32mp1_ddrphy *phy,
+					   char *string, int argc,
+					   char *argv[])
+{
+	u32 rfshctl3 = readl(&ctl->rfshctl3);
+	u32 pwrctl = readl(&ctl->pwrctl);
+	enum test_result res;
+
+	stm32mp1_refresh_disable(ctl);
+	res = read_dqs_gating(ctl, phy, string);
+	stm32mp1_refresh_restore(ctl, rfshctl3, pwrctl);
+
+	return res;
+}
+
+static enum test_result do_bit_deskew(struct stm32mp1_ddrctl *ctl,
+				      struct stm32mp1_ddrphy *phy,
+				      char *string, int argc, char *argv[])
+{
+	u32 rfshctl3 = readl(&ctl->rfshctl3);
+	u32 pwrctl = readl(&ctl->pwrctl);
+	enum test_result res;
+
+	stm32mp1_refresh_disable(ctl);
+	res = bit_deskew(ctl, phy, string);
+	stm32mp1_refresh_restore(ctl, rfshctl3, pwrctl);
+
+	return res;
+}
+
+static enum test_result do_eye_training(struct stm32mp1_ddrctl *ctl,
+					struct stm32mp1_ddrphy *phy,
+					char *string, int argc, char *argv[])
+{
+	u32 rfshctl3 = readl(&ctl->rfshctl3);
+	u32 pwrctl = readl(&ctl->pwrctl);
+	enum test_result res;
+
+	stm32mp1_refresh_disable(ctl);
+	res = eye_training(ctl, phy, string);
+	stm32mp1_refresh_restore(ctl, rfshctl3, pwrctl);
+
+	return res;
+}
+
+static enum test_result do_display(struct stm32mp1_ddrctl *ctl,
+				   struct stm32mp1_ddrphy *phy,
+				   char *string, int argc, char *argv[])
+{
+	int byte;
+	u8 nb_bytes = get_nb_bytes(ctl);
+
+	for (byte = 0; byte < nb_bytes; byte++)
+		display_reg_results(phy, byte);
+
+	return TEST_PASSED;
+}
+
+static enum test_result do_bist_config(struct stm32mp1_ddrctl *ctl,
+				       struct stm32mp1_ddrphy *phy,
+				       char *string, int argc, char *argv[])
+{
+	unsigned long value;
+
+	if (argc > 0) {
+		if (strict_strtoul(argv[0], 0, &value) < 0) {
+			sprintf(string, "invalid nbErr %s", argv[0]);
+			return TEST_FAILED;
+		}
+		BIST_error_max = value;
+	}
+	if (argc > 1) {
+		if (strict_strtoul(argv[1], 0, &value) < 0) {
+			sprintf(string, "invalid Seed %s", argv[1]);
+			return TEST_FAILED;
+		}
+		BIST_seed = value;
+	}
+	printf("Bist.nbErr = %d\n", BIST_error_max);
+	if (BIST_seed)
+		printf("Bist.Seed = 0x%x\n", BIST_seed);
+	else
+		printf("Bist.Seed = random\n");
+
+	return TEST_PASSED;
+}
+
+/****************************************************************
+ * TEST Description
+ ****************************************************************
+ */
+
+const struct test_desc tuning[] = {
+	{do_read_dqs_gating, "Read DQS gating",
+		"software read DQS Gating", "", 0 },
+	{do_bit_deskew, "Bit de-skew", "", "", 0 },
+	{do_eye_training, "Eye Training", "or DQS training", "", 0 },
+	{do_display, "Display registers", "", "", 0 },
+	{do_bist_config, "Bist config", "[nbErr] [seed]",
+	 "configure Bist test", 2},
+};
+
+const int tuning_nb = ARRAY_SIZE(tuning);
diff --git a/drivers/ram/stm32mp1/stm32mp1_tuning.h b/drivers/ram/stm32mp1/stm32mp1_tuning.h
new file mode 100644
index 0000000..964a050
--- /dev/null
+++ b/drivers/ram/stm32mp1/stm32mp1_tuning.h
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef _RAM_STM32MP1_TUNING_H_
+#define _RAM_STM32MP1_TUNING_H_
+
+#define MAX_DQS_PHASE_IDX _144deg
+#define MAX_DQS_UNIT_IDX 7
+#define MAX_GSL_IDX 5
+#define MAX_GPS_IDX 3
+
+/* Number of bytes used in this SW. ( min 1--> max 4). */
+#define NUM_BYTES 4
+
+enum dqs_phase_enum {
+	_36deg = 0,
+	_54deg = 1,
+	_72deg = 2,
+	_90deg = 3,
+	_108deg = 4,
+	_126deg = 5,
+	_144deg = 6
+};
+
+/* BIST Result struct */
+struct BIST_result {
+	/* Overall test result:
+	 * 0 Fail (any bit failed) ,
+	 * 1 Success (All bits success)
+	 */
+	bool test_result;
+	/* 1: true, all fail /  0: False, not all bits fail */
+	bool all_bits_fail;
+	bool bit_i_test_result[8];  /* 0 fail / 1 success */
+};
+
+u8 DQS_phase_index(struct stm32mp1_ddrphy *phy, u8 byte);
+u8 DQS_unit_index(struct stm32mp1_ddrphy *phy, u8 byte);
+u8 DQ_unit_index(struct stm32mp1_ddrphy *phy, u8 byte, u8 bit);
+
+void apply_deskew_results(struct stm32mp1_ddrphy *phy, u8 byte);
+void init_result_struct(struct BIST_result *result);
+void BIST_test(struct stm32mp1_ddrphy *phy, u8 byte,
+	       struct BIST_result *result);
+
+/* a struct that defines tuning parameters of a byte. */
+struct tuning_position {
+	u8 phase; /* DQS phase */
+	u8 unit; /* DQS unit delay */
+	u32 bits_delay; /* Bits deskew in this byte */
+};
+#endif
diff --git a/drivers/remoteproc/Kconfig b/drivers/remoteproc/Kconfig
index 9eb532b..b1cae58 100644
--- a/drivers/remoteproc/Kconfig
+++ b/drivers/remoteproc/Kconfig
@@ -49,4 +49,13 @@ config REMOTEPROC_TI_POWER
 	help
 	  Say 'y' here to add support for TI power processors such as those
 	  found on certain TI keystone and OMAP generation SoCs.
+
+config REMOTEPROC_STM32_COPRO
+	bool "Support for STM32 Co-processor"
+	select REMOTEPROC
+	depends on DM
+	depends on ARCH_STM32MP
+	depends on OF_CONTROL
+	help
+	  Say 'y' here to add support for STM32 Cortex-M4 co-processors.
 endmenu
diff --git a/drivers/remoteproc/Makefile b/drivers/remoteproc/Makefile
index 77eb708..5b120c1 100644
--- a/drivers/remoteproc/Makefile
+++ b/drivers/remoteproc/Makefile
@@ -10,4 +10,5 @@ obj-$(CONFIG_$(SPL_)REMOTEPROC) += rproc-uclass.o
 obj-$(CONFIG_K3_SYSTEM_CONTROLLER) += k3_system_controller.o
 obj-$(CONFIG_REMOTEPROC_K3) += k3_rproc.o
 obj-$(CONFIG_REMOTEPROC_SANDBOX) += sandbox_testproc.o
+obj-$(CONFIG_REMOTEPROC_STM32_COPRO) += stm32_copro.o
 obj-$(CONFIG_REMOTEPROC_TI_POWER) += ti_power_proc.o
diff --git a/drivers/remoteproc/rproc-uclass.c b/drivers/remoteproc/rproc-uclass.c
index c8a41a6..8ea92f7 100644
--- a/drivers/remoteproc/rproc-uclass.c
+++ b/drivers/remoteproc/rproc-uclass.c
@@ -5,6 +5,7 @@
  */
 #define pr_fmt(fmt) "%s: " fmt, __func__
 #include <common.h>
+#include <elf.h>
 #include <errno.h>
 #include <fdtdec.h>
 #include <malloc.h>
@@ -18,6 +19,40 @@
 DECLARE_GLOBAL_DATA_PTR;
 
 /**
+ * struct resource_table - firmware resource table header
+ * @ver: version number
+ * @num: number of resource entries
+ * @reserved: reserved (must be zero)
+ * @offset: array of offsets pointing at the various resource entries
+ *
+ * A resource table is essentially a list of system resources required
+ * by the remote processor. It may also include configuration entries.
+ * If needed, the remote processor firmware should contain this table
+ * as a dedicated ".resource_table" ELF section.
+ *
+ * Some resources entries are mere announcements, where the host is informed
+ * of specific remoteproc configuration. Other entries require the host to
+ * do something (e.g. allocate a system resource). Sometimes a negotiation
+ * is expected, where the firmware requests a resource, and once allocated,
+ * the host should provide back its details (e.g. address of an allocated
+ * memory region).
+ *
+ * The header of the resource table, as expressed by this structure,
+ * contains a version number (should we need to change this format in the
+ * future), the number of available resource entries, and their offsets
+ * in the table.
+ *
+ * Immediately following this header are the resource entries themselves,
+ * each of which begins with a resource entry header (as described below).
+ */
+struct resource_table {
+	u32 ver;
+	u32 num;
+	u32 reserved[2];
+	u32 offset[0];
+} __packed;
+
+/**
  * for_each_remoteproc_device() - iterate through the list of rproc devices
  * @fn: check function to call per match, if this function returns fail,
  *	iteration is aborted with the resultant error value
@@ -291,11 +326,252 @@ int rproc_dev_init(int id)
 	return ret;
 }
 
+/*
+ * Determine if a valid ELF image exists at the given memory location.
+ * First look at the ELF header magic field, then make sure that it is
+ * executable.
+ */
+static bool is_valid_elf_image(unsigned long addr, int size)
+{
+	Elf32_Ehdr *ehdr; /* Elf header structure pointer */
+
+	ehdr = (Elf32_Ehdr *)addr;
+
+	if (!IS_ELF(*ehdr) || size <= sizeof(Elf32_Ehdr)) {
+		pr_err("No elf image at address 0x%08lx\n", addr);
+		return false;
+	}
+
+	if (ehdr->e_type != ET_EXEC) {
+		pr_err("Not a 32-bit elf image at address 0x%08lx\n", addr);
+		return false;
+	}
+
+	return true;
+}
+
+
+/* Basic function to verify ELF image format */
+static int
+rproc_elf_sanity_check(struct udevice *dev, ulong addr, ulong size)
+{
+	Elf32_Ehdr *ehdr;
+	char class;
+
+	if (!addr) {
+		dev_err(dev, "Invalid fw address?\n");
+		return -EINVAL;
+	}
+
+	if (size < sizeof(Elf32_Ehdr)) {
+		dev_err(dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	ehdr = (Elf32_Ehdr *)addr;
+
+	/* We only support ELF32 at this point */
+	class = ehdr->e_ident[EI_CLASS];
+	if (class != ELFCLASS32) {
+		dev_err(dev, "Unsupported class: %d\n", class);
+		return -EINVAL;
+	}
+
+	/* We assume the firmware has the same endianness as the host */
+# ifdef __LITTLE_ENDIAN
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2LSB) {
+# else /* BIG ENDIAN */
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2MSB) {
+# endif
+		dev_err(dev, "Unsupported firmware endianness\n");
+		return -EINVAL;
+	}
+
+	if (size < ehdr->e_shoff + sizeof(Elf32_Shdr)) {
+		dev_err(dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	if (memcmp(ehdr->e_ident, ELFMAG, SELFMAG)) {
+		dev_err(dev, "Image is corrupted (bad magic)\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phnum == 0) {
+		dev_err(dev, "No loadable segments\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phoff > size) {
+		dev_err(dev, "Firmware size is too small\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * A very simple elf loader, assumes the image is valid, returns the
+ * entry point address.
+ */
+static int rproc_load_elf_image(struct udevice *dev, unsigned long addr,
+				unsigned long *entry)
+{
+	Elf32_Ehdr *ehdr; /* Elf header structure pointer */
+	Elf32_Phdr *phdr; /* Program header structure pointer */
+	const struct dm_rproc_ops *ops;
+	unsigned int i;
+
+	ehdr = (Elf32_Ehdr *)addr;
+	phdr = (Elf32_Phdr *)(addr + ehdr->e_phoff);
+
+	ops = rproc_get_ops(dev);
+	if (!ops) {
+		dev_dbg(dev, "Driver has no ops?\n");
+		return -EINVAL;
+	}
+
+	/* Load each program header */
+	for (i = 0; i < ehdr->e_phnum; ++i) {
+		void *dst = (void *)(uintptr_t)phdr->p_paddr;
+		void *src = (void *)addr + phdr->p_offset;
+
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		if (ops->da_to_pa)
+			dst = (void *)ops->da_to_pa(dev, (ulong)dst);
+
+		dev_dbg(dev, "Loading phdr %i to 0x%p (%i bytes)\n",
+			i, dst, phdr->p_filesz);
+		if (phdr->p_filesz)
+			memcpy(dst, src, phdr->p_filesz);
+		if (phdr->p_filesz != phdr->p_memsz)
+			memset(dst + phdr->p_filesz, 0x00,
+			       phdr->p_memsz - phdr->p_filesz);
+		flush_cache((unsigned long)dst, phdr->p_filesz);
+		++phdr;
+	}
+
+	*entry = ehdr->e_entry;
+
+	return 0;
+}
+
+/* Helper to find resource table in an ELF image */
+static Elf32_Shdr *find_rsc_table(struct udevice *dev, Elf32_Ehdr *ehdr,
+				  size_t fw_size)
+{
+	Elf32_Shdr *shdr;
+	int i;
+	const char *name_table;
+	struct resource_table *table = NULL;
+	const u8 *elf_data = (void *)ehdr;
+
+	/* look for the resource table and handle it */
+	shdr = (Elf32_Shdr *)(elf_data + ehdr->e_shoff);
+	name_table = (const char *)(elf_data +
+				    shdr[ehdr->e_shstrndx].sh_offset);
+
+	for (i = 0; i < ehdr->e_shnum; i++, shdr++) {
+		u32 size = shdr->sh_size;
+		u32 offset = shdr->sh_offset;
+
+		if (strcmp(name_table + shdr->sh_name, ".resource_table"))
+			continue;
+
+		table = (struct resource_table *)(elf_data + offset);
+
+		/* make sure we have the entire table */
+		if (offset + size > fw_size || offset + size < size) {
+			dev_err(dev, "resource table truncated\n");
+			return NULL;
+		}
+
+		/* make sure table has at least the header */
+		if (sizeof(struct resource_table) > size) {
+			dev_err(dev, "header-less resource table\n");
+			return NULL;
+		}
+
+		/* we don't support any version beyond the first */
+		if (table->ver != 1) {
+			dev_err(dev, "unsupported fw ver: %d\n", table->ver);
+			return NULL;
+		}
+
+		/* make sure reserved bytes are zeroes */
+		if (table->reserved[0] || table->reserved[1]) {
+			dev_err(dev, "non zero reserved bytes\n");
+			return NULL;
+		}
+
+		/* make sure the offsets array isn't truncated */
+		if (table->num * sizeof(table->offset[0]) +
+				sizeof(struct resource_table) > size) {
+			dev_err(dev, "resource table incomplete\n");
+			return NULL;
+		}
+
+		return shdr;
+	}
+
+	return NULL;
+}
+
+/**
+ * rproc_elf_find_load_rsc_table() - find the loaded resource table
+ * @dev: the rproc device
+ * @fw_addr: the ELF firmware image address
+ * @fw_size: the ELF firmware image size
+ * @rsc_addr: resource table address
+ * @rsc_size: resource table size
+ *
+ * This function finds the resource table, load it and return its address.
+ *
+ * Return: 0 if all ok, else appropriate error value.
+ */
+static int rproc_elf_find_load_rsc_table(struct udevice *dev,
+					 ulong fw_addr, ulong fw_size,
+					 ulong *rsc_addr,
+					 unsigned int *rsc_size)
+{
+	const struct dm_rproc_ops *ops;
+	Elf32_Ehdr *ehdr = (Elf32_Ehdr *)fw_addr;
+	Elf32_Shdr *shdr;
+	void *src;
+	void *dst;
+
+	shdr = find_rsc_table(dev, ehdr, fw_size);
+	if (!shdr)
+		return -ENODATA;
+
+	*rsc_addr = (ulong)shdr->sh_addr;
+	*rsc_size = (unsigned int)shdr->sh_size;
+
+	ops = rproc_get_ops(dev);
+	if (ops->da_to_pa)
+		dst = (void *)ops->da_to_pa(dev, (ulong)shdr->sh_addr);
+	else
+		dst = (void *)shdr->sh_addr;
+
+	dev_dbg(dev, "Loading resource table to 0x%8lx (%i bytes)\n",
+		(ulong)dst, shdr->sh_size);
+
+	src = (void *)fw_addr + shdr->sh_offset;
+
+	memcpy(dst, src, shdr->sh_size);
+	flush_cache((unsigned long)dst, shdr->sh_size);
+
+	return 0;
+}
+
 int rproc_load(int id, ulong addr, ulong size)
 {
 	struct udevice *dev = NULL;
 	struct dm_rproc_uclass_pdata *uc_pdata;
 	const struct dm_rproc_ops *ops;
+	unsigned long entry;
 	int ret;
 
 	ret = uclass_get_device_by_seq(UCLASS_REMOTEPROC, id, &dev);
@@ -315,8 +591,22 @@ int rproc_load(int id, ulong addr, ulong size)
 
 	debug("Loading to '%s' from address 0x%08lX size of %lu bytes\n",
 	      uc_pdata->name, addr, size);
-	if (ops->load)
-		return ops->load(dev, addr, size);
+
+	if (is_valid_elf_image(addr, size)) {
+		if (rproc_elf_sanity_check(dev, addr, size))
+			return -EINVAL;
+
+		/* Elf file load */
+		if (ops->reset)
+			ops->reset(dev);
+
+		return rproc_load_elf_image(dev, addr, &entry);
+
+	} else {
+		/* Binary file load */
+		if (ops->load)
+			return ops->load(dev, addr, size);
+	}
 
 	debug("%s: data corruption?? mandatory function is missing!\n",
 	      dev->name);
@@ -324,6 +614,45 @@ int rproc_load(int id, ulong addr, ulong size)
 	return -EINVAL;
 };
 
+int rproc_load_rsc_table(int id, ulong addr, ulong size, ulong *rsc_addr,
+			 unsigned int *rsc_size)
+{
+	struct udevice *dev = NULL;
+	const struct dm_rproc_ops *ops;
+	int ret;
+
+	ret = uclass_get_device_by_seq(UCLASS_REMOTEPROC, id, &dev);
+	if (ret) {
+		debug("Unknown remote processor id '%d' requested(%d)\n",
+		      id, ret);
+		return -EINVAL;
+	}
+
+	ops = rproc_get_ops(dev);
+	if (!ops) {
+		dev_dbg(dev, "Driver has no ops?\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "Loocking for resource table from address 0x%08lX size of %lu bytes\n",
+		addr, size);
+
+	if (!rproc_elf_sanity_check(dev, addr, size)) {
+		/* load elf image */
+		ret = rproc_elf_find_load_rsc_table(dev, addr, size, rsc_addr,
+						    rsc_size);
+		if (ret) {
+			dev_dbg(dev, "No resource table found\n");
+			return -ENODATA;
+		}
+		dev_dbg(dev, "Resource table at 0x%08lx, size 0x%x!\n",
+			*rsc_addr, *rsc_size);
+		return 0;
+	}
+
+	return -ENODATA;
+};
+
 /*
  * Completely internal helper enums..
  * Keeping this isolated helps this code evolve independent of other
diff --git a/drivers/remoteproc/stm32_copro.c b/drivers/remoteproc/stm32_copro.c
new file mode 100644
index 0000000..310d077
--- /dev/null
+++ b/drivers/remoteproc/stm32_copro.c
@@ -0,0 +1,272 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+#define pr_fmt(fmt) "%s: " fmt, __func__
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <regmap.h>
+#include <remoteproc.h>
+#include <reset.h>
+#include <syscon.h>
+#include <asm/arch/stm32mp1_smc.h>
+
+#define RCC_GCR_HOLD_BOOT	0
+#define RCC_GCR_RELEASE_BOOT	1
+
+/**
+ * struct stm32_copro_privdata - power processor private data
+ * @loadaddr:	base address for loading the power processor
+ * @reset_ctl:	reset controller handle
+ * @hold_boot_regmap
+ * @hold_boot_offset
+ * @hold_boot_mask
+ * @secured_soc:	TZEN flag (register protection)
+ */
+struct stm32_copro_privdata {
+	phys_addr_t loadaddr;
+	struct reset_ctl reset_ctl;
+	struct regmap *hold_boot_regmap;
+	uint hold_boot_offset;
+	uint hold_boot_mask;
+	bool secured_soc;
+	};
+
+/**
+ * st_of_to_priv() - generate private data from device tree
+ * @dev:	corresponding ti remote processor device
+ * @priv:	pointer to driver specific private data
+ *
+ * Return: 0 if all went ok, else corresponding -ve error
+ */
+static int st_of_to_priv(struct udevice *dev,
+			 struct stm32_copro_privdata *priv)
+{
+	struct regmap *regmap;
+	const fdt32_t *cell;
+	const void *blob = gd->fdt_blob;
+	uint tz_offset, tz_mask, tzen;
+	int len, ret;
+
+	if (!blob) {
+		dev_dbg(dev, "no dt?\n");
+		return -EINVAL;
+	}
+
+	priv->loadaddr = dev_read_addr(dev);
+	if (priv->loadaddr == FDT_ADDR_T_NONE) {
+		dev_dbg(dev, "no 'reg' property\n");
+		return -EINVAL;
+	}
+
+	regmap = syscon_phandle_to_regmap(dev, "st,syscfg-holdboot");
+	if (IS_ERR(regmap)) {
+		dev_dbg(dev, "unable to find holdboot regmap (%ld)\n",
+			PTR_ERR(regmap));
+		return -EINVAL;
+	}
+
+	priv->hold_boot_regmap = regmap;
+
+	cell = dev_read_prop(dev, "st,syscfg-holdboot", &len);
+	if (len < 3 * sizeof(fdt32_t)) {
+		dev_dbg(dev, "holdboot offset and mask not available\n");
+		return -EINVAL;
+	}
+
+	priv->hold_boot_offset = fdtdec_get_number(cell + 1, 1);
+
+	priv->hold_boot_mask = fdtdec_get_number(cell + 2, 1);
+
+	regmap = syscon_phandle_to_regmap(dev, "st,syscfg-tz");
+	if (IS_ERR(regmap)) {
+		dev_dbg(dev, "unable to find tz regmap (%ld)\n",
+			PTR_ERR(regmap));
+		return -EINVAL;
+	}
+
+	cell = dev_read_prop(dev, "st,syscfg-tz", &len);
+	if (len < 3 * sizeof(fdt32_t)) {
+		dev_dbg(dev, "tz offset and mask not available\n");
+		return -EINVAL;
+	}
+
+	tz_offset = fdtdec_get_number(cell + 1, 1);
+
+	tz_mask = fdtdec_get_number(cell + 2, 1);
+
+	ret = regmap_read(regmap, tz_offset, &tzen);
+	if (ret) {
+		dev_dbg(dev, "failed to read soc secure state\n");
+		return ret;
+	}
+
+	priv->secured_soc = !!(tzen & tz_mask);
+
+	return reset_get_by_index(dev, 0, &priv->reset_ctl);
+}
+
+/**
+ * stm32_copro_probe() - Basic probe
+ * @dev:	corresponding STM32 remote processor device
+ *
+ * Return: 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_probe(struct udevice *dev)
+{
+	struct stm32_copro_privdata *priv;
+	int ret;
+
+	priv = dev_get_priv(dev);
+
+	ret = st_of_to_priv(dev, priv);
+
+	dev_dbg(dev, "probed with slave_addr=0x%08lX (%d)\n",
+		priv->loadaddr, ret);
+
+	return ret;
+}
+
+/* Hold boot bit management */
+static int stm32_copro_set_hold_boot(struct udevice *dev, bool hold)
+{
+	struct stm32_copro_privdata *priv;
+	uint status, val;
+	int ret;
+
+	priv = dev_get_priv(dev);
+
+	val = hold ? RCC_GCR_HOLD_BOOT : RCC_GCR_RELEASE_BOOT;
+
+	if (priv->secured_soc) {
+		return stm32_smc_exec(STM32_SMC_RCC, STM32_SMC_REG_WRITE,
+				      priv->hold_boot_offset, val);
+	}
+
+	ret = regmap_read(priv->hold_boot_regmap, priv->hold_boot_offset,
+			  &status);
+	if (ret) {
+		dev_err(dev, "failed to read status of mcu\n");
+		return ret;
+	}
+
+	status &= ~priv->hold_boot_mask;
+	status |= val;
+
+	ret = regmap_write(priv->hold_boot_regmap, priv->hold_boot_offset,
+			   status);
+	if (ret)
+		dev_err(dev, "failed to set hold boot\n");
+
+	return ret;
+}
+
+/**
+ * stm32_copro_load() - Loadup the STM32 Cortex-M4 remote processor
+ * @dev:	corresponding STM32 remote processor device
+ * @addr:	Address in memory where image binary is stored
+ * @size:	Size in bytes of the image binary
+ *
+ * Return: 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_load(struct udevice *dev, ulong addr, ulong size)
+{
+	struct stm32_copro_privdata *priv;
+	int ret;
+
+	priv = dev_get_priv(dev);
+
+	stm32_copro_set_hold_boot(dev, true);
+
+	ret = reset_assert(&priv->reset_ctl);
+	if (ret) {
+		dev_dbg(dev, "Unable assert reset line (ret=%d)\n", ret);
+		return ret;
+	}
+
+	dev_dbg(dev, "Loading binary from 0x%08lX, size 0x%08lX to 0x%08lX\n",
+		addr, size, priv->loadaddr);
+
+	memcpy((void *)priv->loadaddr, (void *)addr, size);
+
+	dev_dbg(dev, "Complete!\n");
+	return 0;
+}
+
+/**
+ * stm32_copro_start() - Start Cortex-M4 coprocessor
+ * @dev:	corresponding st remote processor device
+ *
+ * Return: 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_start(struct udevice *dev)
+{
+	int ret;
+
+	/* move hold boot from true to false start the copro */
+	ret = stm32_copro_set_hold_boot(dev, false);
+	if (ret)
+		return ret;
+
+	/*
+	 * Once copro running, reset hold boot flag to avoid copro
+	 * rebooting autonomously
+	 */
+	return stm32_copro_set_hold_boot(dev, true);
+}
+
+/**
+ * stm32_copro_reset() - Reset Cortex-M4 coprocessor
+ * @dev:	corresponding st remote processor device
+ *
+ * Return: 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_reset(struct udevice *dev)
+{
+	struct stm32_copro_privdata *priv;
+	int ret;
+
+	priv = dev_get_priv(dev);
+
+	stm32_copro_set_hold_boot(dev, true);
+
+	ret = reset_assert(&priv->reset_ctl);
+	if (ret) {
+		dev_dbg(dev, "Unable assert reset line (ret=%d)\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+ulong stm32_copro_da_to_pa(struct udevice *dev, ulong da)
+{
+	/* to update according to lastest DT */
+	if (da >= 0 && da < 0x10000)
+		return (da + 0x38000000);
+
+	return da;
+}
+
+static const struct dm_rproc_ops stm32_copro_ops = {
+	.load = stm32_copro_load,
+	.start = stm32_copro_start,
+	.reset = stm32_copro_reset,
+	.da_to_pa = stm32_copro_da_to_pa,
+};
+
+static const struct udevice_id stm32_copro_ids[] = {
+	{.compatible = "st,stm32mp1-rproc"},
+	{}
+};
+
+U_BOOT_DRIVER(stm32_copro) = {
+	.name = "stm32_m4_proc",
+	.of_match = stm32_copro_ids,
+	.id = UCLASS_REMOTEPROC,
+	.ops = &stm32_copro_ops,
+	.probe = stm32_copro_probe,
+	.priv_auto_alloc_size = sizeof(struct stm32_copro_privdata),
+};
diff --git a/drivers/reset/stm32-reset.c b/drivers/reset/stm32-reset.c
index 16d3dba..1d00e17 100644
--- a/drivers/reset/stm32-reset.c
+++ b/drivers/reset/stm32-reset.c
@@ -11,6 +11,13 @@
 #include <stm32_rcc.h>
 #include <asm/io.h>
 
+#ifdef CONFIG_STM32MP1_TRUSTED
+#include <asm/arch/stm32mp1_smc.h>
+
+#define RCC_APB5RST_BANK 0x62
+#define RCC_AHB5RST_BANK 0x64
+#endif /* CONFIG_STM32MP1_TRUSTED */
+
 /* reset clear offset for STM32MP RCC */
 #define RCC_CL 0x4
 
@@ -33,12 +40,23 @@ static int stm32_reset_assert(struct reset_ctl *reset_ctl)
 	struct stm32_reset_priv *priv = dev_get_priv(reset_ctl->dev);
 	int bank = (reset_ctl->id / BITS_PER_LONG) * 4;
 	int offset = reset_ctl->id % BITS_PER_LONG;
+#ifdef CONFIG_STM32MP1_TRUSTED
+	int rcc_bank = reset_ctl->id / BITS_PER_LONG;
+#endif /* CONFIG_STM32MP1_TRUSTED */
+
 	debug("%s: reset id = %ld bank = %d offset = %d)\n", __func__,
 	      reset_ctl->id, bank, offset);
 
 	if (dev_get_driver_data(reset_ctl->dev) == STM32MP1)
 		/* reset assert is done in rcc set register */
-		writel(BIT(offset), priv->base + bank);
+#ifdef CONFIG_STM32MP1_TRUSTED
+		if (rcc_bank == RCC_APB5RST_BANK ||
+		    rcc_bank == RCC_AHB5RST_BANK)
+			stm32_smc_exec(STM32_SMC_RCC, STM32_SMC_REG_WRITE,
+				       bank, BIT(offset));
+		else
+#endif /* CONFIG_STM32MP1_TRUSTED */
+			writel(BIT(offset), priv->base + bank);
 	else
 		setbits_le32(priv->base + bank, BIT(offset));
 
@@ -50,12 +68,23 @@ static int stm32_reset_deassert(struct reset_ctl *reset_ctl)
 	struct stm32_reset_priv *priv = dev_get_priv(reset_ctl->dev);
 	int bank = (reset_ctl->id / BITS_PER_LONG) * 4;
 	int offset = reset_ctl->id % BITS_PER_LONG;
+#ifdef CONFIG_STM32MP1_TRUSTED
+	int rcc_bank = reset_ctl->id / BITS_PER_LONG;
+#endif /* CONFIG_STM32MP1_TRUSTED */
+
 	debug("%s: reset id = %ld bank = %d offset = %d)\n", __func__,
 	      reset_ctl->id, bank, offset);
 
 	if (dev_get_driver_data(reset_ctl->dev) == STM32MP1)
 		/* reset deassert is done in rcc clr register */
-		writel(BIT(offset), priv->base + bank + RCC_CL);
+#ifdef CONFIG_STM32MP1_TRUSTED
+		if (rcc_bank == RCC_APB5RST_BANK ||
+		    rcc_bank == RCC_AHB5RST_BANK)
+			stm32_smc_exec(STM32_SMC_RCC, STM32_SMC_REG_WRITE,
+				       bank + RCC_CL, BIT(offset));
+		else
+#endif /* CONFIG_STM32MP1_TRUSTED */
+			writel(BIT(offset), priv->base + bank + RCC_CL);
 	else
 		clrbits_le32(priv->base + bank, BIT(offset));
 
diff --git a/drivers/serial/serial_stm32.c b/drivers/serial/serial_stm32.c
index 66e02d5..d806057 100644
--- a/drivers/serial/serial_stm32.c
+++ b/drivers/serial/serial_stm32.c
@@ -7,6 +7,7 @@
 #include <common.h>
 #include <clk.h>
 #include <dm.h>
+#include <reset.h>
 #include <serial.h>
 #include <watchdog.h>
 #include <asm/io.h>
@@ -66,6 +67,14 @@ static int stm32_serial_setconfig(struct udevice *dev, uint serial_config)
 	if (bits != SERIAL_8_BITS || stop != SERIAL_ONE_STOP || stm32f4)
 		return -ENOTSUPP; /* not supported in driver*/
 
+	/*
+	 * only parity config is implemented, check if other serial settings
+	 * are the default one.
+	 * (STM32F4 serial IP didn't support parity setting)
+	 */
+	if (bits != SERIAL_8_BITS || stop != SERIAL_ONE_STOP || stm32f4)
+		return -ENOTSUPP; /* not supported in driver*/
+
 	clrbits_le32(cr1, USART_CR1_RE | USART_CR1_TE | BIT(uart_enable_bit));
 	/* update usart configuration (uart need to be disable)
 	 * PCE: parity check enable
@@ -171,6 +180,7 @@ static int stm32_serial_probe(struct udevice *dev)
 {
 	struct stm32x7_serial_platdata *plat = dev_get_platdata(dev);
 	struct clk clk;
+	struct reset_ctl reset;
 	int ret;
 
 	plat->uart_info = (struct stm32_uart_info *)dev_get_driver_data(dev);
@@ -185,6 +195,13 @@ static int stm32_serial_probe(struct udevice *dev)
 		return ret;
 	}
 
+	ret = reset_get_by_index(dev, 0, &reset);
+	if (!ret) {
+		reset_assert(&reset);
+		udelay(2);
+		reset_deassert(&reset);
+	}
+
 	plat->clock_rate = clk_get_rate(&clk);
 	if (plat->clock_rate < 0) {
 		clk_disable(&clk);
@@ -258,7 +275,6 @@ static inline void _debug_uart_init(void)
 	_stm32_serial_setbrg(base, uart_info,
 			     CONFIG_DEBUG_UART_CLOCK,
 			     CONFIG_BAUDRATE);
-	printf("DEBUG done\n");
 }
 
 static inline void _debug_uart_putc(int c)
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 516188e..da96754 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -191,7 +191,7 @@ config SANDBOX_SPI
 
 config STM32_QSPI
 	bool "STM32F7 QSPI driver"
-	depends on STM32F7
+	depends on STM32F7 || ARCH_STM32MP
 	help
 	  Enable the STM32F7 Quad-SPI (QSPI) driver. This driver can be
 	  used to access the SPI NOR flash chips on platforms embedding
diff --git a/drivers/spi/soft_spi.c b/drivers/spi/soft_spi.c
index b06883f..b80f810 100644
--- a/drivers/spi/soft_spi.c
+++ b/drivers/spi/soft_spi.c
@@ -215,8 +215,8 @@ static int soft_spi_probe(struct udevice *dev)
 	int cs_flags, clk_flags;
 	int ret;
 
-	cs_flags = (slave->mode & SPI_CS_HIGH) ? 0 : GPIOD_ACTIVE_LOW;
-	clk_flags = (slave->mode & SPI_CPOL) ? GPIOD_ACTIVE_LOW : 0;
+	cs_flags = (slave && slave->mode & SPI_CS_HIGH) ? 0 : GPIOD_ACTIVE_LOW;
+	clk_flags = (slave && slave->mode & SPI_CPOL) ? GPIOD_ACTIVE_LOW : 0;
 
 	if (gpio_request_by_name(dev, "cs-gpios", 0, &plat->cs,
 				 GPIOD_IS_OUT | cs_flags) ||
diff --git a/drivers/sysreset/sysreset_syscon.c b/drivers/sysreset/sysreset_syscon.c
index 3450640..9d02aaf 100644
--- a/drivers/sysreset/sysreset_syscon.c
+++ b/drivers/sysreset/sysreset_syscon.c
@@ -24,6 +24,9 @@ static int syscon_reboot_request(struct udevice *dev, enum sysreset_t type)
 {
 	struct syscon_reboot_priv *priv = dev_get_priv(dev);
 
+	if (type == SYSRESET_POWER)
+		return -EPROTONOSUPPORT;
+
 	regmap_write(priv->regmap, priv->offset, priv->mask);
 
 	return -EINPROGRESS;
@@ -36,20 +39,9 @@ static struct sysreset_ops syscon_reboot_ops = {
 int syscon_reboot_probe(struct udevice *dev)
 {
 	struct syscon_reboot_priv *priv = dev_get_priv(dev);
-	int err;
-	u32 phandle;
-	ofnode node;
-
-	err = ofnode_read_u32(dev_ofnode(dev), "regmap", &phandle);
-	if (err)
-		return err;
-
-	node = ofnode_get_by_phandle(phandle);
-	if (!ofnode_valid(node))
-		return -EINVAL;
 
-	priv->regmap = syscon_node_to_regmap(node);
-	if (!priv->regmap) {
+	priv->regmap = syscon_phandle_to_regmap(dev, "regmap");
+	if (IS_ERR(priv->regmap)) {
 		pr_err("unable to find regmap\n");
 		return -ENODEV;
 	}
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 26b4d12..fa1cdc4 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -72,6 +72,15 @@ config USB_GADGET_BCM_UDC_OTG_PHY
 	help
 	  Enable the Broadcom UDC OTG physical device interface.
 
+config USB_GADGET_GEN_UDC_OTG_PHY
+	bool "Generic UDC OTG PHY"
+	depends on PHY && USB_GADGET_DWC2_OTG
+	default y if ARCH_STM32MP
+	help
+	  Enable the generic UDC OTG physical device interface.
+	  it is based on the generic phy driver API on the phy
+	  u-class and on the device tree information
+
 config USB_GADGET_DWC2_OTG
 	bool "DesignWare USB2.0 HS OTG controller (gadget mode)"
 	select USB_GADGET_DUALSPEED
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index b74c1fd..abb3c20 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_USB_GADGET_BCM_UDC_OTG_PHY) += bcm_udc_otg_phy.o
 obj-$(CONFIG_USB_GADGET_DWC2_OTG) += dwc2_udc_otg.o
 obj-$(CONFIG_USB_GADGET_DWC2_OTG_PHY) += dwc2_udc_otg_phy.o
 obj-$(CONFIG_USB_GADGET_FOTG210) += fotg210.o
+obj-$(CONFIG_USB_GADGET_GEN_UDC_OTG_PHY) += gen_udc_otg_phy.o
 obj-$(CONFIG_CI_UDC)	+= ci_udc.o
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_USB_GADGET_DOWNLOAD) += g_dnl.o
diff --git a/drivers/usb/gadget/dwc2_udc_otg.c b/drivers/usb/gadget/dwc2_udc_otg.c
index e3edd10..6552a5b 100644
--- a/drivers/usb/gadget/dwc2_udc_otg.c
+++ b/drivers/usb/gadget/dwc2_udc_otg.c
@@ -425,6 +425,9 @@ static void reconfig_usbd(struct dwc2_udc *dev)
 
 	writel(dflt_gusbcfg, &reg->gusbcfg);
 
+	if (dev->pdata->usb_gotgctl)
+		setbits_le32(&reg->gotgctl, dev->pdata->usb_gotgctl);
+
 	/* 3. Put the OTG device core in the disconnected state.*/
 	uTemp = readl(&reg->dctl);
 	uTemp |= SOFT_DISCONNECT;
diff --git a/drivers/usb/gadget/g_dnl.c b/drivers/usb/gadget/g_dnl.c
index e9e1600..43d0516 100644
--- a/drivers/usb/gadget/g_dnl.c
+++ b/drivers/usb/gadget/g_dnl.c
@@ -89,6 +89,11 @@ static struct usb_gadget_strings *g_dnl_composite_strings[] = {
 	NULL,
 };
 
+void g_dnl_set_product(const char *s)
+{
+	g_dnl_string_defs[1].s = s;
+}
+
 static int g_dnl_unbind(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget *gadget = cdev->gadget;
diff --git a/drivers/usb/gadget/gen_udc_otg_phy.c b/drivers/usb/gadget/gen_udc_otg_phy.c
new file mode 100644
index 0000000..36c31cd
--- /dev/null
+++ b/drivers/usb/gadget/gen_udc_otg_phy.c
@@ -0,0 +1,66 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <generic-phy.h>
+#include <dm/uclass.h>
+
+#include "dwc2_udc_otg_priv.h"
+
+void otg_phy_init(struct dwc2_udc *dev)
+{
+	struct dwc2_plat_otg_data *pdata = dev->pdata;
+	struct udevice *phy_dev;
+	struct phy phy;
+
+	if (uclass_get_device_by_of_offset(UCLASS_PHY,
+					   pdata->phy_of_node, &phy_dev)) {
+		pr_err("failed to found usb phy\n");
+		hang();
+		return;
+	}
+
+	phy.dev = phy_dev;
+	phy.id = pdata->regs_phy;
+
+	if (generic_phy_init(&phy)) {
+		pr_err("failed to init usb phy\n");
+		generic_phy_power_off(&phy);
+		return;
+	}
+
+	if (generic_phy_power_on(&phy)) {
+		pr_err("unable to power on the phy\n");
+		return;
+	}
+
+	pr_debug("USB Generic PHY Enable\n");
+}
+
+void otg_phy_off(struct dwc2_udc *dev)
+{
+	struct dwc2_plat_otg_data *pdata = dev->pdata;
+	struct udevice *phy_dev;
+	struct phy phy;
+	int ret;
+
+	uclass_get_device_by_of_offset(UCLASS_PHY,
+				       pdata->phy_of_node, &phy_dev);
+	phy.dev = phy_dev;
+	phy.id = pdata->regs_phy;
+
+	ret = generic_phy_power_off(&phy);
+	if (ret) {
+		pr_err("failed to power off usb phy\n");
+		return;
+	}
+	ret = generic_phy_exit(&phy);
+	if (ret) {
+		pr_err("failed to power off usb phy\n");
+		return;
+	}
+
+	pr_debug("USB Generic PHY Disable\n");
+}
diff --git a/drivers/usb/host/dwc2.c b/drivers/usb/host/dwc2.c
index b6f008a..f96da8e 100644
--- a/drivers/usb/host/dwc2.c
+++ b/drivers/usb/host/dwc2.c
@@ -5,12 +5,15 @@
  */
 
 #include <common.h>
+#include <clk.h>
 #include <dm.h>
 #include <errno.h>
-#include <usb.h>
+#include <generic-phy.h>
 #include <malloc.h>
 #include <memalign.h>
 #include <phys2bus.h>
+#include <reset.h>
+#include <usb.h>
 #include <usbroothubdes.h>
 #include <wait_bit.h>
 #include <asm/io.h>
@@ -35,6 +38,7 @@ struct dwc2_priv {
 #ifdef CONFIG_DM_REGULATOR
 	struct udevice *vbus_supply;
 #endif
+	struct phy phy;
 #else
 	uint8_t *aligned_buffer;
 	uint8_t *status_buffer;
@@ -811,7 +815,7 @@ int wait_for_chhltd(struct dwc2_hc_regs *hc_regs, uint32_t *sub, u8 *toggle)
 	uint32_t hcint, hctsiz;
 
 	ret = wait_for_bit_le32(&hc_regs->hcint, DWC2_HCINT_CHHLTD, true,
-				2000, false);
+			   2000, false);
 	if (ret)
 		return ret;
 
@@ -1210,6 +1214,8 @@ static int dwc2_init_common(struct udevice *dev, struct dwc2_priv *priv)
 	if (readl(&regs->gintsts) & DWC2_GINTSTS_CURMODE_HOST)
 		mdelay(1000);
 
+	printf("USB DWC2\n");
+
 	return 0;
 }
 
@@ -1317,13 +1323,119 @@ static int dwc2_usb_ofdata_to_platdata(struct udevice *dev)
 	return 0;
 }
 
+static int dwc2_setup_phy(struct udevice *dev)
+{
+	struct dwc2_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	ret = generic_phy_get_by_index(dev, 0, &priv->phy);
+	if (ret) {
+		if (ret != -ENOENT) {
+			dev_err(dev, "failed to get usb phy\n");
+			return ret;
+		}
+		return 0;
+	}
+
+	ret = generic_phy_init(&priv->phy);
+	if (ret) {
+		dev_err(dev, "failed to init usb phy\n");
+		return ret;
+	}
+
+	ret = generic_phy_power_on(&priv->phy);
+	if (ret) {
+		dev_err(dev, "failed to power on usb phy\n");
+		return generic_phy_exit(&priv->phy);
+	}
+
+	return 0;
+}
+
+static int dwc2_shutdown_phy(struct udevice *dev)
+{
+	struct dwc2_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	if (!generic_phy_valid(&priv->phy))
+		return 0;
+
+	ret = generic_phy_power_off(&priv->phy);
+	if (ret) {
+		dev_err(dev, "failed to power off usb phy\n");
+		return ret;
+	}
+
+	ret = generic_phy_exit(&priv->phy);
+	if (ret) {
+		dev_err(dev, "failed to power off usb phy\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int dwc2_reset_init(struct udevice *dev)
+{
+	struct reset_ctl_bulk	resets;
+	int ret;
+
+	ret = reset_get_bulk(dev, &resets);
+	if (ret == -ENOTSUPP || ret == -ENOENT)
+		return 0;
+	else if (ret)
+		return ret;
+
+	reset_assert_bulk(&resets);
+	udelay(2);
+	ret = reset_deassert_bulk(&resets);
+	if (ret) {
+		reset_release_bulk(&resets);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dwc2_clk_init(struct udevice *dev)
+{
+	struct clk_bulk		clks;
+	int ret;
+
+	ret = clk_get_bulk(dev, &clks);
+	if (ret == -ENOSYS || ret == -ENOENT)
+		return 0;
+	if (ret)
+		return ret;
+
+	ret = clk_enable_bulk(&clks);
+	if (ret) {
+		clk_release_bulk(&clks);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int dwc2_usb_probe(struct udevice *dev)
 {
 	struct dwc2_priv *priv = dev_get_priv(dev);
 	struct usb_bus_priv *bus_priv = dev_get_uclass_priv(dev);
+	int ret;
 
 	bus_priv->desc_before_addr = true;
 
+	ret = dwc2_reset_init(dev);
+	if (ret)
+		return ret;
+
+	ret = dwc2_clk_init(dev);
+	if (ret)
+		return ret;
+
+	ret = dwc2_setup_phy(dev);
+	if (ret)
+		return ret;
+
 	return dwc2_init_common(dev, priv);
 }
 
@@ -1336,6 +1448,8 @@ static int dwc2_usb_remove(struct udevice *dev)
 	if (ret)
 		return ret;
 
+	dwc2_shutdown_phy(dev);
+
 	dwc2_uninit_common(priv->regs);
 
 	reset_release_bulk(&priv->resets);
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 25c94f4..8809552 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -73,6 +73,13 @@ config VIDEO_ANSI
 	  Enable ANSI escape sequence decoding for a more fully functional
 	  console.
 
+config VIDEO_MIPI_DSI
+	bool
+	help
+	  Support MIPI DSI interface for driving a MIPI compatible device.
+	  The MIPI Display Serial Interface (MIPI DSI) defines a high-speed
+	  serial interface between a host processor and a display module.
+
 config CONSOLE_NORMAL
 	bool "Support a simple text console"
 	depends on DM_VIDEO
@@ -320,6 +327,22 @@ config VIDEO_LCD_ANX9804
 	from a parallel LCD interface and translate it on the fy into a DP
 	interface for driving eDP TFT displays. It uses I2C for configuration.
 
+config VIDEO_LCD_ORISETECH_OTM8009A
+	bool "OTM8009A DSI LCD panel support"
+	depends on DM_VIDEO
+	select VIDEO_MIPI_DSI
+	default n
+	---help---
+	Support for Orise Tech otm8009a 480p dsi 2dl video mode panel.
+
+config VIDEO_LCD_RAYDIUM_RM68200
+	bool "RM68200 DSI LCD panel support"
+	depends on DM_VIDEO
+	select VIDEO_MIPI_DSI
+	default n
+	---help---
+	Support for Raydium rm68200 720x1280 dsi 2dl video mode panel.
+
 config VIDEO_LCD_SSD2828
 	bool "SSD2828 bridge chip"
 	default n
@@ -681,6 +704,16 @@ config VIDEO_DW_HDMI
 	  rather requires a SoC-specific glue driver to call it), it
 	  can not be enabled from the configuration menu.
 
+config VIDEO_DW_MIPI_DSI
+	bool
+	select VIDEO_MIPI_DSI
+	help
+	  Enables the common driver code for the Synopsis Designware
+	  block found in SoCs from various vendors.
+	  As this does not provide any functionality by itself (but
+	  rather requires a SoC-specific glue driver to call it), it
+	  can not be enabled from the configuration menu.
+
 config VIDEO_SIMPLE
 	bool "Simple display driver for preconfigured display"
 	help
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 80e1e82..c3d39ed 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -50,6 +50,8 @@ obj-$(CONFIG_VIDEO_IPUV3) += mxc_ipuv3_fb.o ipu_common.o ipu_disp.o
 obj-$(CONFIG_VIDEO_IVYBRIDGE_IGD) += ivybridge_igd.o
 obj-$(CONFIG_VIDEO_LCD_ANX9804) += anx9804.o
 obj-$(CONFIG_VIDEO_LCD_HITACHI_TX18D42VM) += hitachi_tx18d42vm_lcd.o
+obj-$(CONFIG_VIDEO_LCD_ORISETECH_OTM8009A) += orisetech_otm8009a.o
+obj-$(CONFIG_VIDEO_LCD_RAYDIUM_RM68200) += raydium-rm68200.o
 obj-$(CONFIG_VIDEO_LCD_SSD2828) += ssd2828.o
 obj-$(CONFIG_VIDEO_MB862xx) += mb862xx.o videomodes.o
 obj-$(CONFIG_VIDEO_MVEBU) += mvebu_lcd.o
@@ -61,6 +63,8 @@ obj-$(CONFIG_VIDEO_SIMPLE) += simplefb.o
 obj-$(CONFIG_VIDEO_TEGRA20) += tegra.o
 obj-$(CONFIG_VIDEO_VCXK) += bus_vcxk.o
 obj-$(CONFIG_VIDEO_VESA) += vesa.o
+obj-$(CONFIG_VIDEO_DW_MIPI_DSI) += dw_mipi_dsi.o
+obj-${CONFIG_VIDEO_MIPI_DSI} += mipi_display.o
 
 obj-y += bridge/
 obj-y += sunxi/
diff --git a/drivers/video/dw_mipi_dsi.c b/drivers/video/dw_mipi_dsi.c
new file mode 100644
index 0000000..fe1e25a
--- /dev/null
+++ b/drivers/video/dw_mipi_dsi.c
@@ -0,0 +1,826 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2016, Fuzhou Rockchip Electronics Co., Ltd
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ * Author(s): Philippe Cornu <philippe.cornu@st.com> for STMicroelectronics.
+ *            Yannick Fertre <yannick.fertre@st.com> for STMicroelectronics.
+ *
+ * This generic Synopsys DesignWare MIPI DSI host driver is based on the
+ * Linux Kernel driver from drivers/gpu/drm/bridge/synopsys/dw-mipi-dsi.c.
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <errno.h>
+#include <mipi_display.h>
+#include <panel.h>
+#include <video.h>
+#include <asm/io.h>
+#include <asm/arch/gpio.h>
+#include <dm/device-internal.h>
+#include <dw_mipi_dsi.h>
+#include <linux/iopoll.h>
+#include <video_bridge.h>
+
+#define HWVER_131			0x31333100	/* IP version 1.31 */
+
+#define DSI_VERSION			0x00
+#define VERSION				GENMASK(31, 8)
+
+#define DSI_PWR_UP			0x04
+#define RESET				0
+#define POWERUP				BIT(0)
+
+#define DSI_CLKMGR_CFG			0x08
+#define TO_CLK_DIVISION(div)		(((div) & 0xff) << 8)
+#define TX_ESC_CLK_DIVISION(div)	((div) & 0xff)
+
+#define DSI_DPI_VCID			0x0c
+#define DPI_VCID(vcid)			((vcid) & 0x3)
+
+#define DSI_DPI_COLOR_CODING		0x10
+#define LOOSELY18_EN			BIT(8)
+#define DPI_COLOR_CODING_16BIT_1	0x0
+#define DPI_COLOR_CODING_16BIT_2	0x1
+#define DPI_COLOR_CODING_16BIT_3	0x2
+#define DPI_COLOR_CODING_18BIT_1	0x3
+#define DPI_COLOR_CODING_18BIT_2	0x4
+#define DPI_COLOR_CODING_24BIT		0x5
+
+#define DSI_DPI_CFG_POL			0x14
+#define COLORM_ACTIVE_LOW		BIT(4)
+#define SHUTD_ACTIVE_LOW		BIT(3)
+#define HSYNC_ACTIVE_LOW		BIT(2)
+#define VSYNC_ACTIVE_LOW		BIT(1)
+#define DATAEN_ACTIVE_LOW		BIT(0)
+
+#define DSI_DPI_LP_CMD_TIM		0x18
+#define OUTVACT_LPCMD_TIME(p)		(((p) & 0xff) << 16)
+#define INVACT_LPCMD_TIME(p)		((p) & 0xff)
+
+#define DSI_DBI_VCID			0x1c
+#define DSI_DBI_CFG			0x20
+#define DSI_DBI_PARTITIONING_EN		0x24
+#define DSI_DBI_CMDSIZE			0x28
+
+#define DSI_PCKHDL_CFG			0x2c
+#define CRC_RX_EN			BIT(4)
+#define ECC_RX_EN			BIT(3)
+#define BTA_EN				BIT(2)
+#define EOTP_RX_EN			BIT(1)
+#define EOTP_TX_EN			BIT(0)
+
+#define DSI_GEN_VCID			0x30
+
+#define DSI_MODE_CFG			0x34
+#define ENABLE_VIDEO_MODE		0
+#define ENABLE_CMD_MODE			BIT(0)
+
+#define DSI_VID_MODE_CFG		0x38
+#define ENABLE_LOW_POWER		(0x3f << 8)
+#define ENABLE_LOW_POWER_MASK		(0x3f << 8)
+#define VID_MODE_TYPE_NON_BURST_SYNC_PULSES	0x0
+#define VID_MODE_TYPE_NON_BURST_SYNC_EVENTS	0x1
+#define VID_MODE_TYPE_BURST			0x2
+#define VID_MODE_TYPE_MASK			0x3
+
+#define DSI_VID_PKT_SIZE		0x3c
+#define VID_PKT_SIZE(p)			((p) & 0x3fff)
+
+#define DSI_VID_NUM_CHUNKS		0x40
+#define VID_NUM_CHUNKS(c)		((c) & 0x1fff)
+
+#define DSI_VID_NULL_SIZE		0x44
+#define VID_NULL_SIZE(b)		((b) & 0x1fff)
+
+#define DSI_VID_HSA_TIME		0x48
+#define DSI_VID_HBP_TIME		0x4c
+#define DSI_VID_HLINE_TIME		0x50
+#define DSI_VID_VSA_LINES		0x54
+#define DSI_VID_VBP_LINES		0x58
+#define DSI_VID_VFP_LINES		0x5c
+#define DSI_VID_VACTIVE_LINES		0x60
+#define DSI_EDPI_CMD_SIZE		0x64
+
+#define DSI_CMD_MODE_CFG		0x68
+#define MAX_RD_PKT_SIZE_LP		BIT(24)
+#define DCS_LW_TX_LP			BIT(19)
+#define DCS_SR_0P_TX_LP			BIT(18)
+#define DCS_SW_1P_TX_LP			BIT(17)
+#define DCS_SW_0P_TX_LP			BIT(16)
+#define GEN_LW_TX_LP			BIT(14)
+#define GEN_SR_2P_TX_LP			BIT(13)
+#define GEN_SR_1P_TX_LP			BIT(12)
+#define GEN_SR_0P_TX_LP			BIT(11)
+#define GEN_SW_2P_TX_LP			BIT(10)
+#define GEN_SW_1P_TX_LP			BIT(9)
+#define GEN_SW_0P_TX_LP			BIT(8)
+#define ACK_RQST_EN			BIT(1)
+#define TEAR_FX_EN			BIT(0)
+
+#define CMD_MODE_ALL_LP			(MAX_RD_PKT_SIZE_LP | \
+					 DCS_LW_TX_LP | \
+					 DCS_SR_0P_TX_LP | \
+					 DCS_SW_1P_TX_LP | \
+					 DCS_SW_0P_TX_LP | \
+					 GEN_LW_TX_LP | \
+					 GEN_SR_2P_TX_LP | \
+					 GEN_SR_1P_TX_LP | \
+					 GEN_SR_0P_TX_LP | \
+					 GEN_SW_2P_TX_LP | \
+					 GEN_SW_1P_TX_LP | \
+					 GEN_SW_0P_TX_LP)
+
+#define DSI_GEN_HDR			0x6c
+#define DSI_GEN_PLD_DATA		0x70
+
+#define DSI_CMD_PKT_STATUS		0x74
+#define GEN_RD_CMD_BUSY			BIT(6)
+#define GEN_PLD_R_FULL			BIT(5)
+#define GEN_PLD_R_EMPTY			BIT(4)
+#define GEN_PLD_W_FULL			BIT(3)
+#define GEN_PLD_W_EMPTY			BIT(2)
+#define GEN_CMD_FULL			BIT(1)
+#define GEN_CMD_EMPTY			BIT(0)
+
+#define DSI_TO_CNT_CFG			0x78
+#define HSTX_TO_CNT(p)			(((p) & 0xffff) << 16)
+#define LPRX_TO_CNT(p)			((p) & 0xffff)
+
+#define DSI_HS_RD_TO_CNT		0x7c
+#define DSI_LP_RD_TO_CNT		0x80
+#define DSI_HS_WR_TO_CNT		0x84
+#define DSI_LP_WR_TO_CNT		0x88
+#define DSI_BTA_TO_CNT			0x8c
+
+#define DSI_LPCLK_CTRL			0x94
+#define AUTO_CLKLANE_CTRL		BIT(1)
+#define PHY_TXREQUESTCLKHS		BIT(0)
+
+#define DSI_PHY_TMR_LPCLK_CFG		0x98
+#define PHY_CLKHS2LP_TIME(lbcc)		(((lbcc) & 0x3ff) << 16)
+#define PHY_CLKLP2HS_TIME(lbcc)		((lbcc) & 0x3ff)
+
+#define DSI_PHY_TMR_CFG			0x9c
+#define PHY_HS2LP_TIME(lbcc)		(((lbcc) & 0xff) << 24)
+#define PHY_LP2HS_TIME(lbcc)		(((lbcc) & 0xff) << 16)
+#define MAX_RD_TIME(lbcc)		((lbcc) & 0x7fff)
+#define PHY_HS2LP_TIME_V131(lbcc)	(((lbcc) & 0x3ff) << 16)
+#define PHY_LP2HS_TIME_V131(lbcc)	((lbcc) & 0x3ff)
+
+#define DSI_PHY_RSTZ			0xa0
+#define PHY_DISFORCEPLL			0
+#define PHY_ENFORCEPLL			BIT(3)
+#define PHY_DISABLECLK			0
+#define PHY_ENABLECLK			BIT(2)
+#define PHY_RSTZ			0
+#define PHY_UNRSTZ			BIT(1)
+#define PHY_SHUTDOWNZ			0
+#define PHY_UNSHUTDOWNZ			BIT(0)
+
+#define DSI_PHY_IF_CFG			0xa4
+#define PHY_STOP_WAIT_TIME(cycle)	(((cycle) & 0xff) << 8)
+#define N_LANES(n)			(((n) - 1) & 0x3)
+
+#define DSI_PHY_ULPS_CTRL		0xa8
+#define DSI_PHY_TX_TRIGGERS		0xac
+
+#define DSI_PHY_STATUS			0xb0
+#define PHY_STOP_STATE_CLK_LANE		BIT(2)
+#define PHY_LOCK			BIT(0)
+
+#define DSI_PHY_TST_CTRL0		0xb4
+#define PHY_TESTCLK			BIT(1)
+#define PHY_UNTESTCLK			0
+#define PHY_TESTCLR			BIT(0)
+#define PHY_UNTESTCLR			0
+
+#define DSI_PHY_TST_CTRL1		0xb8
+#define PHY_TESTEN			BIT(16)
+#define PHY_UNTESTEN			0
+#define PHY_TESTDOUT(n)			(((n) & 0xff) << 8)
+#define PHY_TESTDIN(n)			((n) & 0xff)
+
+#define DSI_INT_ST0			0xbc
+#define DSI_INT_ST1			0xc0
+#define DSI_INT_MSK0			0xc4
+#define DSI_INT_MSK1			0xc8
+
+#define DSI_PHY_TMR_RD_CFG		0xf4
+#define MAX_RD_TIME_V131(lbcc)		((lbcc) & 0x7fff)
+
+#define PHY_STATUS_TIMEOUT_US		10000
+#define CMD_PKT_STATUS_TIMEOUT_US	20000
+
+#define MSEC_PER_SEC			1000
+
+struct dw_mipi_dsi {
+	struct mipi_dsi_host dsi_host;
+	struct mipi_dsi_device *device;
+	void __iomem *base;
+	unsigned int lane_mbps; /* per lane */
+	u32 channel;
+	u32 lanes;
+	u32 format;
+	unsigned long mode_flags;
+	unsigned int max_data_lanes;
+	const struct dw_mipi_dsi_phy_ops *phy_ops;
+};
+
+static int dsi_mode_vrefresh(struct display_timing *timings)
+{
+	int refresh = 0;
+	unsigned int calc_val;
+	u32 htotal = timings->hactive.typ + timings->hfront_porch.typ +
+		     timings->hback_porch.typ + timings->hsync_len.typ;
+	u32 vtotal = timings->vactive.typ + timings->vfront_porch.typ +
+		     timings->vback_porch.typ + timings->vsync_len.typ;
+
+	if (htotal > 0 && vtotal > 0) {
+		calc_val = timings->pixelclock.typ;
+		calc_val /= htotal;
+		refresh = (calc_val + vtotal / 2) / vtotal;
+	}
+
+	return refresh;
+}
+
+/*
+ * The controller should generate 2 frames before
+ * preparing the peripheral.
+ */
+static void dw_mipi_dsi_wait_for_two_frames(struct display_timing *timings)
+{
+	int refresh, two_frames;
+
+	refresh = dsi_mode_vrefresh(timings);
+	two_frames = DIV_ROUND_UP(MSEC_PER_SEC, refresh) * 2;
+	mdelay(two_frames);
+}
+
+static inline struct dw_mipi_dsi *host_to_dsi(struct mipi_dsi_host *host)
+{
+	return container_of(host, struct dw_mipi_dsi, dsi_host);
+}
+
+static inline void dsi_write(struct dw_mipi_dsi *dsi, u32 reg, u32 val)
+{
+	writel(val, dsi->base + reg);
+}
+
+static inline u32 dsi_read(struct dw_mipi_dsi *dsi, u32 reg)
+{
+	return readl(dsi->base + reg);
+}
+
+static int dw_mipi_dsi_host_attach(struct mipi_dsi_host *host,
+				   struct mipi_dsi_device *device)
+{
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+
+	if (device->lanes > dsi->max_data_lanes) {
+		dev_err(device->dev,
+			"the number of data lanes(%u) is too many\n",
+			device->lanes);
+		return -EINVAL;
+	}
+
+	dsi->lanes = device->lanes;
+	dsi->channel = device->channel;
+	dsi->format = device->format;
+	dsi->mode_flags = device->mode_flags;
+
+	return 0;
+}
+
+static void dw_mipi_message_config(struct dw_mipi_dsi *dsi,
+				   const struct mipi_dsi_msg *msg)
+{
+	bool lpm = msg->flags & MIPI_DSI_MSG_USE_LPM;
+	u32 val = 0;
+
+	if (msg->flags & MIPI_DSI_MSG_REQ_ACK)
+		val |= ACK_RQST_EN;
+	if (lpm)
+		val |= CMD_MODE_ALL_LP;
+
+	dsi_write(dsi, DSI_LPCLK_CTRL, lpm ? 0 : PHY_TXREQUESTCLKHS);
+	dsi_write(dsi, DSI_CMD_MODE_CFG, val);
+}
+
+static int dw_mipi_dsi_gen_pkt_hdr_write(struct dw_mipi_dsi *dsi, u32 hdr_val)
+{
+	int ret;
+	u32 val, mask;
+
+	ret = readl_poll_timeout(dsi->base + DSI_CMD_PKT_STATUS,
+				 val, !(val & GEN_CMD_FULL),
+				 CMD_PKT_STATUS_TIMEOUT_US);
+	if (ret) {
+		dev_err(dsi->dev, "failed to get available command FIFO\n");
+		return ret;
+	}
+
+	dsi_write(dsi, DSI_GEN_HDR, hdr_val);
+
+	mask = GEN_CMD_EMPTY | GEN_PLD_W_EMPTY;
+	ret = readl_poll_timeout(dsi->base + DSI_CMD_PKT_STATUS,
+				 val, (val & mask) == mask,
+				 CMD_PKT_STATUS_TIMEOUT_US);
+	if (ret) {
+		dev_err(dsi->dev, "failed to write command FIFO\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dw_mipi_dsi_write(struct dw_mipi_dsi *dsi,
+			     const struct mipi_dsi_packet *packet)
+{
+	const u8 *tx_buf = packet->payload;
+	int len = packet->payload_length, pld_data_bytes = sizeof(u32), ret;
+	__le32 word;
+	u32 val;
+
+	while (len) {
+		if (len < pld_data_bytes) {
+			word = 0;
+			memcpy(&word, tx_buf, len);
+			dsi_write(dsi, DSI_GEN_PLD_DATA, le32_to_cpu(word));
+			len = 0;
+		} else {
+			memcpy(&word, tx_buf, pld_data_bytes);
+			dsi_write(dsi, DSI_GEN_PLD_DATA, le32_to_cpu(word));
+			tx_buf += pld_data_bytes;
+			len -= pld_data_bytes;
+		}
+
+		ret = readl_poll_timeout(dsi->base + DSI_CMD_PKT_STATUS,
+					 val, !(val & GEN_PLD_W_FULL),
+					 CMD_PKT_STATUS_TIMEOUT_US);
+		if (ret) {
+			dev_err(dsi->dev,
+				"failed to get available write payload FIFO\n");
+			return ret;
+		}
+	}
+
+	word = 0;
+	memcpy(&word, packet->header, sizeof(packet->header));
+	return dw_mipi_dsi_gen_pkt_hdr_write(dsi, le32_to_cpu(word));
+}
+
+static int dw_mipi_dsi_read(struct dw_mipi_dsi *dsi,
+			    const struct mipi_dsi_msg *msg)
+{
+	int i, j, ret, len = msg->rx_len;
+	u8 *buf = msg->rx_buf;
+	u32 val;
+
+	/* Wait end of the read operation */
+	ret = readl_poll_timeout(dsi->base + DSI_CMD_PKT_STATUS,
+				 val, !(val & GEN_RD_CMD_BUSY),
+				 CMD_PKT_STATUS_TIMEOUT_US);
+	if (ret) {
+		dev_err(dsi->dev, "Timeout during read operation\n");
+		return ret;
+	}
+
+	for (i = 0; i < len; i += 4) {
+		/* Read fifo must not be empty before all bytes are read */
+		ret = readl_poll_timeout(dsi->base + DSI_CMD_PKT_STATUS,
+					 val, !(val & GEN_PLD_R_EMPTY),
+					 CMD_PKT_STATUS_TIMEOUT_US);
+		if (ret) {
+			dev_err(dsi->dev, "Read payload FIFO is empty\n");
+			return ret;
+		}
+
+		val = dsi_read(dsi, DSI_GEN_PLD_DATA);
+		for (j = 0; j < 4 && j + i < len; j++)
+			buf[i + j] = val >> (8 * j);
+	}
+
+	return ret;
+}
+
+static ssize_t dw_mipi_dsi_host_transfer(struct mipi_dsi_host *host,
+					 const struct mipi_dsi_msg *msg)
+{
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+	struct mipi_dsi_packet packet;
+	int ret, nb_bytes;
+
+	ret = mipi_dsi_create_packet(&packet, msg);
+	if (ret) {
+		dev_err(dsi->dev, "failed to create packet: %d\n", ret);
+		return ret;
+	}
+
+	dw_mipi_message_config(dsi, msg);
+
+	ret = dw_mipi_dsi_write(dsi, &packet);
+	if (ret)
+		return ret;
+
+	if (msg->rx_buf && msg->rx_len) {
+		ret = dw_mipi_dsi_read(dsi, msg);
+		if (ret)
+			return ret;
+		nb_bytes = msg->rx_len;
+	} else {
+		nb_bytes = packet.size;
+	}
+
+	return nb_bytes;
+}
+
+static const struct mipi_dsi_host_ops dw_mipi_dsi_host_ops = {
+	.attach = dw_mipi_dsi_host_attach,
+	.transfer = dw_mipi_dsi_host_transfer,
+};
+
+static void dw_mipi_dsi_video_mode_config(struct dw_mipi_dsi *dsi)
+{
+	u32 val;
+
+	/*
+	 * TODO dw drv improvements
+	 * enabling low power is panel-dependent, we should use the
+	 * panel configuration here...
+	 */
+	val = ENABLE_LOW_POWER;
+
+	if (dsi->mode_flags & MIPI_DSI_MODE_VIDEO_BURST)
+		val |= VID_MODE_TYPE_BURST;
+	else if (dsi->mode_flags & MIPI_DSI_MODE_VIDEO_SYNC_PULSE)
+		val |= VID_MODE_TYPE_NON_BURST_SYNC_PULSES;
+	else
+		val |= VID_MODE_TYPE_NON_BURST_SYNC_EVENTS;
+
+	dsi_write(dsi, DSI_VID_MODE_CFG, val);
+}
+
+static void dw_mipi_dsi_set_mode(struct dw_mipi_dsi *dsi,
+				 unsigned long mode_flags)
+{
+	dsi_write(dsi, DSI_PWR_UP, RESET);
+
+	if (mode_flags & MIPI_DSI_MODE_VIDEO) {
+		dsi_write(dsi, DSI_MODE_CFG, ENABLE_VIDEO_MODE);
+		dw_mipi_dsi_video_mode_config(dsi);
+		dsi_write(dsi, DSI_LPCLK_CTRL, PHY_TXREQUESTCLKHS);
+	} else {
+		dsi_write(dsi, DSI_MODE_CFG, ENABLE_CMD_MODE);
+	}
+
+	dsi_write(dsi, DSI_PWR_UP, POWERUP);
+}
+
+static void dw_mipi_dsi_init(struct dw_mipi_dsi *dsi)
+{
+	/*
+	 * The maximum permitted escape clock is 20MHz and it is derived from
+	 * lanebyteclk, which is running at "lane_mbps / 8".  Thus we want:
+	 *
+	 *     (lane_mbps >> 3) / esc_clk_division < 20
+	 * which is:
+	 *     (lane_mbps >> 3) / 20 > esc_clk_division
+	 */
+	u32 esc_clk_division = (dsi->lane_mbps >> 3) / 20 + 1;
+
+	dsi_write(dsi, DSI_PWR_UP, RESET);
+
+	/*
+	 * TODO dw drv improvements
+	 * timeout clock division should be computed with the
+	 * high speed transmission counter timeout and byte lane...
+	 */
+	dsi_write(dsi, DSI_CLKMGR_CFG, TO_CLK_DIVISION(10) |
+		  TX_ESC_CLK_DIVISION(esc_clk_division));
+}
+
+static void dw_mipi_dsi_dpi_config(struct dw_mipi_dsi *dsi,
+				   struct display_timing *timings)
+{
+	u32 val = 0, color = 0;
+
+	switch (dsi->format) {
+	case MIPI_DSI_FMT_RGB888:
+		color = DPI_COLOR_CODING_24BIT;
+		break;
+	case MIPI_DSI_FMT_RGB666:
+		color = DPI_COLOR_CODING_18BIT_2 | LOOSELY18_EN;
+		break;
+	case MIPI_DSI_FMT_RGB666_PACKED:
+		color = DPI_COLOR_CODING_18BIT_1;
+		break;
+	case MIPI_DSI_FMT_RGB565:
+		color = DPI_COLOR_CODING_16BIT_1;
+		break;
+	}
+
+	if (dsi->mode_flags & DISPLAY_FLAGS_VSYNC_HIGH)
+		val |= VSYNC_ACTIVE_LOW;
+	if (dsi->mode_flags & DISPLAY_FLAGS_HSYNC_HIGH)
+		val |= HSYNC_ACTIVE_LOW;
+
+	dsi_write(dsi, DSI_DPI_VCID, DPI_VCID(dsi->channel));
+	dsi_write(dsi, DSI_DPI_COLOR_CODING, color);
+	dsi_write(dsi, DSI_DPI_CFG_POL, val);
+	/*
+	 * TODO dw drv improvements
+	 * largest packet sizes during hfp or during vsa/vpb/vfp
+	 * should be computed according to byte lane, lane number and only
+	 * if sending lp cmds in high speed is enable (PHY_TXREQUESTCLKHS)
+	 */
+	dsi_write(dsi, DSI_DPI_LP_CMD_TIM, OUTVACT_LPCMD_TIME(4)
+		  | INVACT_LPCMD_TIME(4));
+}
+
+static void dw_mipi_dsi_packet_handler_config(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PCKHDL_CFG, CRC_RX_EN | ECC_RX_EN | BTA_EN);
+}
+
+static void dw_mipi_dsi_video_packet_config(struct dw_mipi_dsi *dsi,
+					    struct display_timing *timings)
+{
+	/*
+	 * TODO dw drv improvements
+	 * only burst mode is supported here. For non-burst video modes,
+	 * we should compute DSI_VID_PKT_SIZE, DSI_VCCR.NUMC &
+	 * DSI_VNPCR.NPSIZE... especially because this driver supports
+	 * non-burst video modes, see dw_mipi_dsi_video_mode_config()...
+	 */
+	dsi_write(dsi, DSI_VID_PKT_SIZE, VID_PKT_SIZE(timings->hactive.typ));
+}
+
+static void dw_mipi_dsi_command_mode_config(struct dw_mipi_dsi *dsi)
+{
+	/*
+	 * TODO dw drv improvements
+	 * compute high speed transmission counter timeout according
+	 * to the timeout clock division (TO_CLK_DIVISION) and byte lane...
+	 */
+	dsi_write(dsi, DSI_TO_CNT_CFG, HSTX_TO_CNT(1000) | LPRX_TO_CNT(1000));
+	/*
+	 * TODO dw drv improvements
+	 * the Bus-Turn-Around Timeout Counter should be computed
+	 * according to byte lane...
+	 */
+	dsi_write(dsi, DSI_BTA_TO_CNT, 0xd00);
+	dsi_write(dsi, DSI_MODE_CFG, ENABLE_CMD_MODE);
+}
+
+/* Get lane byte clock cycles. */
+static u32 dw_mipi_dsi_get_hcomponent_lbcc(struct dw_mipi_dsi *dsi,
+					   struct display_timing *timings,
+					   u32 hcomponent)
+{
+	u32 frac, lbcc;
+
+	lbcc = hcomponent * dsi->lane_mbps * MSEC_PER_SEC / 8;
+
+	frac = lbcc % (timings->pixelclock.typ / 1000);
+	lbcc = lbcc / (timings->pixelclock.typ / 1000);
+	if (frac)
+		lbcc++;
+
+	return lbcc;
+}
+
+static void dw_mipi_dsi_line_timer_config(struct dw_mipi_dsi *dsi,
+					  struct display_timing *timings)
+{
+	u32 htotal, hsa, hbp, lbcc;
+
+	htotal = timings->hactive.typ + timings->hfront_porch.typ +
+		 timings->hback_porch.typ + timings->hsync_len.typ;
+
+	hsa = timings->hback_porch.typ;
+	hbp = timings->hsync_len.typ;
+
+	/*
+	 * TODO dw drv improvements
+	 * computations below may be improved...
+	 */
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, timings, htotal);
+	dsi_write(dsi, DSI_VID_HLINE_TIME, lbcc);
+
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, timings, hsa);
+	dsi_write(dsi, DSI_VID_HSA_TIME, lbcc);
+
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, timings, hbp);
+	dsi_write(dsi, DSI_VID_HBP_TIME, lbcc);
+}
+
+static void dw_mipi_dsi_vertical_timing_config(struct dw_mipi_dsi *dsi,
+					       struct display_timing *timings)
+{
+	u32 vactive, vsa, vfp, vbp;
+
+	vactive = timings->vactive.typ;
+	vsa =  timings->vback_porch.typ;
+	vfp =  timings->vfront_porch.typ;
+	vbp = timings->vsync_len.typ;
+
+	dsi_write(dsi, DSI_VID_VACTIVE_LINES, vactive);
+	dsi_write(dsi, DSI_VID_VSA_LINES, vsa);
+	dsi_write(dsi, DSI_VID_VFP_LINES, vfp);
+	dsi_write(dsi, DSI_VID_VBP_LINES, vbp);
+}
+
+static void dw_mipi_dsi_dphy_timing_config(struct dw_mipi_dsi *dsi)
+{
+	u32 hw_version;
+
+	/*
+	 * TODO dw drv improvements
+	 * data & clock lane timers should be computed according to panel
+	 * blankings and to the automatic clock lane control mode...
+	 * note: DSI_PHY_TMR_CFG.MAX_RD_TIME should be in line with
+	 * DSI_CMD_MODE_CFG.MAX_RD_PKT_SIZE_LP (see CMD_MODE_ALL_LP)
+	 */
+
+	hw_version = dsi_read(dsi, DSI_VERSION) & VERSION;
+
+	if (hw_version >= HWVER_131) {
+		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME_V131(0x40) |
+			  PHY_LP2HS_TIME_V131(0x40));
+		dsi_write(dsi, DSI_PHY_TMR_RD_CFG, MAX_RD_TIME_V131(10000));
+	} else {
+		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME(0x40) |
+			  PHY_LP2HS_TIME(0x40) | MAX_RD_TIME(10000));
+	}
+
+	dsi_write(dsi, DSI_PHY_TMR_LPCLK_CFG, PHY_CLKHS2LP_TIME(0x40)
+		  | PHY_CLKLP2HS_TIME(0x40));
+}
+
+static void dw_mipi_dsi_dphy_interface_config(struct dw_mipi_dsi *dsi)
+{
+	/*
+	 * TODO dw drv improvements
+	 * stop wait time should be the maximum between host dsi
+	 * and panel stop wait times
+	 */
+	dsi_write(dsi, DSI_PHY_IF_CFG, PHY_STOP_WAIT_TIME(0x20) |
+		  N_LANES(dsi->lanes));
+}
+
+static void dw_mipi_dsi_dphy_init(struct dw_mipi_dsi *dsi)
+{
+	/* Clear PHY state */
+	dsi_write(dsi, DSI_PHY_RSTZ, PHY_DISFORCEPLL | PHY_DISABLECLK
+		  | PHY_RSTZ | PHY_SHUTDOWNZ);
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_UNTESTCLR);
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_TESTCLR);
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_UNTESTCLR);
+}
+
+static void dw_mipi_dsi_dphy_enable(struct dw_mipi_dsi *dsi)
+{
+	u32 val;
+	int ret;
+
+	dsi_write(dsi, DSI_PHY_RSTZ, PHY_ENFORCEPLL | PHY_ENABLECLK |
+		  PHY_UNRSTZ | PHY_UNSHUTDOWNZ);
+
+	ret = readl_poll_timeout(dsi->base + DSI_PHY_STATUS, val,
+				 val & PHY_LOCK, PHY_STATUS_TIMEOUT_US);
+	if (ret)
+		dev_warn(dsi->dev, "failed to wait phy lock state\n");
+
+	ret = readl_poll_timeout(dsi->base + DSI_PHY_STATUS,
+				 val, val & PHY_STOP_STATE_CLK_LANE,
+				 PHY_STATUS_TIMEOUT_US);
+	if (ret)
+		dev_warn(dsi->dev, "failed to wait phy clk lane stop state\n");
+}
+
+static void dw_mipi_dsi_clear_err(struct dw_mipi_dsi *dsi)
+{
+	dsi_read(dsi, DSI_INT_ST0);
+	dsi_read(dsi, DSI_INT_ST1);
+	dsi_write(dsi, DSI_INT_MSK0, 0);
+	dsi_write(dsi, DSI_INT_MSK1, 0);
+}
+
+static void dw_mipi_dsi_bridge_set(struct dw_mipi_dsi *dsi,
+				   struct display_timing *timings)
+{
+	const struct dw_mipi_dsi_phy_ops *phy_ops = dsi->phy_ops;
+	int ret;
+
+	ret = phy_ops->get_lane_mbps(dsi->device, timings, dsi->lanes,
+				     dsi->format, &dsi->lane_mbps);
+	if (ret)
+		dev_warn(dsi->dev, "Phy get_lane_mbps() failed\n");
+
+	dw_mipi_dsi_init(dsi);
+	dw_mipi_dsi_dpi_config(dsi, timings);
+	dw_mipi_dsi_packet_handler_config(dsi);
+	dw_mipi_dsi_video_mode_config(dsi);
+	dw_mipi_dsi_video_packet_config(dsi, timings);
+	dw_mipi_dsi_command_mode_config(dsi);
+	dw_mipi_dsi_line_timer_config(dsi, timings);
+	dw_mipi_dsi_vertical_timing_config(dsi, timings);
+
+	dw_mipi_dsi_dphy_init(dsi);
+	dw_mipi_dsi_dphy_timing_config(dsi);
+	dw_mipi_dsi_dphy_interface_config(dsi);
+
+	dw_mipi_dsi_clear_err(dsi);
+
+	ret = phy_ops->init(dsi->device);
+	if (ret)
+		dev_warn(dsi->dev, "Phy init() failed\n");
+
+	dw_mipi_dsi_dphy_enable(dsi);
+
+	dw_mipi_dsi_wait_for_two_frames(timings);
+
+	/* Switch to cmd mode for panel-bridge pre_enable & panel prepare */
+	dw_mipi_dsi_set_mode(dsi, 0);
+}
+
+void dw_mipi_dsi_bridge_enable(struct mipi_dsi_device *device)
+{
+	struct mipi_dsi_host *host = device->host;
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+
+	/* Switch to video mode for panel-bridge enable & panel enable */
+	dw_mipi_dsi_set_mode(dsi, MIPI_DSI_MODE_VIDEO);
+}
+EXPORT_SYMBOL_GPL(dw_mipi_dsi_bridge_enable);
+
+int dw_mipi_dsi_init_bridge(struct mipi_dsi_device *device)
+{
+	struct dw_mipi_dsi_plat_data *platdata = dev_get_platdata(device->dev);
+	struct udevice *panel = platdata->panel;
+	struct display_timing timings;
+	struct dw_mipi_dsi *dsi;
+	struct clk clk;
+	int ret;
+
+	dsi = kzalloc(sizeof(*dsi), GFP_KERNEL);
+
+	dsi->phy_ops = platdata->phy_ops;
+	dsi->max_data_lanes = platdata->max_data_lanes;
+	dsi->device = device;
+	dsi->dsi_host.ops = &dw_mipi_dsi_host_ops;
+	device->host = &dsi->dsi_host;
+
+	 /* TODO Get these settings from panel */
+	dsi->lanes = 2;
+	dsi->format = MIPI_DSI_FMT_RGB888;
+	dsi->mode_flags = MIPI_DSI_MODE_VIDEO |
+			  MIPI_DSI_MODE_VIDEO_BURST |
+			  MIPI_DSI_MODE_LPM;
+
+	dsi->base = (void *)dev_read_addr(device->dev);
+	if ((fdt_addr_t)dsi->base == FDT_ADDR_T_NONE) {
+		dev_err(device->dev, "dsi dt register address error\n");
+		return -EINVAL;
+	}
+
+	ret = panel_get_display_timing(panel, &timings);
+	if (ret) {
+		ret = fdtdec_decode_display_timing(gd->fdt_blob,
+						   dev_of_offset(panel),
+						   0, &timings);
+		if (ret) {
+			dev_err(dev, "decode display timing error %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (!dsi->phy_ops->init || !dsi->phy_ops->get_lane_mbps) {
+		dev_err(device->dev, "Phy not properly configured\n");
+		return -ENODEV;
+	}
+
+	ret = clk_get_by_name(device->dev, "px_clk", &clk);
+	if (ret) {
+		dev_err(device->dev, "peripheral clock get error %d\n", ret);
+		return ret;
+	}
+
+	/*  get the pixel clock set by the clock framework */
+	timings.pixelclock.typ = clk_get_rate(&clk);
+
+	dw_mipi_dsi_bridge_set(dsi, &timings);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dw_mipi_dsi_init_bridge);
+
+MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
+MODULE_AUTHOR("Philippe Cornu <philippe.cornu@st.com>");
+MODULE_AUTHOR("Yannick Fertré <yannick.fertre@st.com>");
+MODULE_DESCRIPTION("DW MIPI DSI host controller driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:dw-mipi-dsi");
diff --git a/drivers/video/mipi_display.c b/drivers/video/mipi_display.c
new file mode 100644
index 0000000..611ee53
--- /dev/null
+++ b/drivers/video/mipi_display.c
@@ -0,0 +1,817 @@
+/*
+ * MIPI DSI Bus
+ *
+ * Copyright (C) 2012-2013, Samsung Electronics, Co., Ltd.
+ * Copyright (C) 2018 STMicroelectronics - All Rights Reserved
+ * Andrzej Hajda <a.hajda@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Mipi_display.c contains a set of dsi helpers.
+ * This file is based on the drm helper file drivers/gpu/drm/drm_mipi_dsi.c
+ * (kernel linux).
+ *
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <display.h>
+#include <dm.h>
+#include <mipi_display.h>
+
+/**
+ * DOC: dsi helpers
+ *
+ * These functions contain some common logic and helpers to deal with MIPI DSI
+ * peripherals.
+ *
+ * Helpers are provided for a number of standard MIPI DSI command as well as a
+ * subset of the MIPI DCS command set.
+ */
+
+/**
+ * mipi_dsi_attach - attach a DSI device to its DSI host
+ * @dsi: DSI peripheral
+ */
+int mipi_dsi_attach(struct mipi_dsi_device *dsi)
+{
+	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
+
+	if (!ops || !ops->attach)
+		return -ENOSYS;
+
+	return ops->attach(dsi->host, dsi);
+}
+EXPORT_SYMBOL(mipi_dsi_attach);
+
+/**
+ * mipi_dsi_detach - detach a DSI device from its DSI host
+ * @dsi: DSI peripheral
+ */
+int mipi_dsi_detach(struct mipi_dsi_device *dsi)
+{
+	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
+
+	if (!ops || !ops->detach)
+		return -ENOSYS;
+
+	return ops->detach(dsi->host, dsi);
+}
+EXPORT_SYMBOL(mipi_dsi_detach);
+
+/**
+ * mipi_dsi_device_transfer - transfer message to a DSI device
+ * @dsi: DSI peripheral
+ * @msg: message
+ */
+static ssize_t mipi_dsi_device_transfer(struct mipi_dsi_device *dsi,
+					struct mipi_dsi_msg *msg)
+{
+	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
+
+	if (!ops || !ops->transfer)
+		return -ENOSYS;
+
+	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
+		msg->flags |= MIPI_DSI_MSG_USE_LPM;
+
+	return ops->transfer(dsi->host, msg);
+}
+
+/**
+ * mipi_dsi_packet_format_is_short - check if a packet is of the short format
+ * @type: MIPI DSI data type of the packet
+ *
+ * Return: true if the packet for the given data type is a short packet, false
+ * otherwise.
+ */
+bool mipi_dsi_packet_format_is_short(u8 type)
+{
+	switch (type) {
+	case MIPI_DSI_V_SYNC_START:
+	case MIPI_DSI_V_SYNC_END:
+	case MIPI_DSI_H_SYNC_START:
+	case MIPI_DSI_H_SYNC_END:
+	case MIPI_DSI_END_OF_TRANSMISSION:
+	case MIPI_DSI_COLOR_MODE_OFF:
+	case MIPI_DSI_COLOR_MODE_ON:
+	case MIPI_DSI_SHUTDOWN_PERIPHERAL:
+	case MIPI_DSI_TURN_ON_PERIPHERAL:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM:
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_DCS_READ:
+	case MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE:
+		return true;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL(mipi_dsi_packet_format_is_short);
+
+/**
+ * mipi_dsi_packet_format_is_long - check if a packet is of the long format
+ * @type: MIPI DSI data type of the packet
+ *
+ * Return: true if the packet for the given data type is a long packet, false
+ * otherwise.
+ */
+bool mipi_dsi_packet_format_is_long(u8 type)
+{
+	switch (type) {
+	case MIPI_DSI_NULL_PACKET:
+	case MIPI_DSI_BLANKING_PACKET:
+	case MIPI_DSI_GENERIC_LONG_WRITE:
+	case MIPI_DSI_DCS_LONG_WRITE:
+	case MIPI_DSI_LOOSELY_PACKED_PIXEL_STREAM_YCBCR20:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR24:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_30:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_36:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_16:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_18:
+	case MIPI_DSI_PIXEL_STREAM_3BYTE_18:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_24:
+		return true;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL(mipi_dsi_packet_format_is_long);
+
+/**
+ * mipi_dsi_create_packet - create a packet from a message according to the
+ *     DSI protocol
+ * @packet: pointer to a DSI packet structure
+ * @msg: message to translate into a packet
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_create_packet(struct mipi_dsi_packet *packet,
+			   const struct mipi_dsi_msg *msg)
+{
+	if (!packet || !msg)
+		return -EINVAL;
+
+	/* do some minimum sanity checking */
+	if (!mipi_dsi_packet_format_is_short(msg->type) &&
+	    !mipi_dsi_packet_format_is_long(msg->type))
+		return -EINVAL;
+
+	if (msg->channel > 3)
+		return -EINVAL;
+
+	memset(packet, 0, sizeof(*packet));
+	packet->header[0] = ((msg->channel & 0x3) << 6) | (msg->type & 0x3f);
+
+	/* TODO: compute ECC if hardware support is not available */
+
+	/*
+	 * Long write packets contain the word count in header bytes 1 and 2.
+	 * The payload follows the header and is word count bytes long.
+	 *
+	 * Short write packets encode up to two parameters in header bytes 1
+	 * and 2.
+	 */
+	if (mipi_dsi_packet_format_is_long(msg->type)) {
+		packet->header[1] = (msg->tx_len >> 0) & 0xff;
+		packet->header[2] = (msg->tx_len >> 8) & 0xff;
+
+		packet->payload_length = msg->tx_len;
+		packet->payload = msg->tx_buf;
+	} else {
+		const u8 *tx = msg->tx_buf;
+
+		packet->header[1] = (msg->tx_len > 0) ? tx[0] : 0;
+		packet->header[2] = (msg->tx_len > 1) ? tx[1] : 0;
+	}
+
+	packet->size = sizeof(packet->header) + packet->payload_length;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_create_packet);
+
+/*
+ * mipi_dsi_set_maximum_return_packet_size() - specify the maximum size of the
+ *    the payload in a long packet transmitted from the peripheral back to the
+ *    host processor
+ * @dsi: DSI peripheral device
+ * @value: the maximum size of the payload
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_set_maximum_return_packet_size(struct mipi_dsi_device *dsi,
+					    u16 value)
+{
+	u8 tx[2] = { value & 0xff, value >> 8 };
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.type = MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE,
+		.tx_len = sizeof(tx),
+		.tx_buf = tx,
+	};
+	int ret = mipi_dsi_device_transfer(dsi, &msg);
+
+	return (ret < 0) ? ret : 0;
+}
+EXPORT_SYMBOL(mipi_dsi_set_maximum_return_packet_size);
+
+/**
+ * mipi_dsi_generic_write() - transmit data using a generic write packet
+ * @dsi: DSI peripheral device
+ * @payload: buffer containing the payload
+ * @size: size of payload buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the payload length.
+ *
+ * Return: The number of bytes transmitted on success or a negative error code
+ * on failure.
+ */
+ssize_t mipi_dsi_generic_write(struct mipi_dsi_device *dsi, const void *payload,
+			       size_t size)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.tx_buf = payload,
+		.tx_len = size
+	};
+
+	switch (size) {
+	case 0:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM;
+		break;
+
+	case 1:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM;
+		break;
+
+	case 2:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM;
+		break;
+
+	default:
+		msg.type = MIPI_DSI_GENERIC_LONG_WRITE;
+		break;
+	}
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_generic_write);
+
+/**
+ * mipi_dsi_generic_read() - receive data using a generic read packet
+ * @dsi: DSI peripheral device
+ * @params: buffer containing the request parameters
+ * @num_params: number of request parameters
+ * @data: buffer in which to return the received data
+ * @size: size of receive buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the number of parameters passed in.
+ *
+ * Return: The number of bytes successfully read or a negative error code on
+ * failure.
+ */
+ssize_t mipi_dsi_generic_read(struct mipi_dsi_device *dsi, const void *params,
+			      size_t num_params, void *data, size_t size)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.tx_len = num_params,
+		.tx_buf = params,
+		.rx_len = size,
+		.rx_buf = data
+	};
+
+	switch (num_params) {
+	case 0:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM;
+		break;
+
+	case 1:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM;
+		break;
+
+	case 2:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_generic_read);
+
+/**
+ * mipi_dsi_dcs_write_buffer() - transmit a DCS command with payload
+ * @dsi: DSI peripheral device
+ * @data: buffer containing data to be transmitted
+ * @len: size of transmission buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the command payload length.
+ *
+ * Return: The number of bytes successfully transmitted or a negative error
+ * code on failure.
+ */
+ssize_t mipi_dsi_dcs_write_buffer(struct mipi_dsi_device *dsi,
+				  const void *data, size_t len)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.tx_buf = data,
+		.tx_len = len
+	};
+
+	switch (len) {
+	case 0:
+		return -EINVAL;
+
+	case 1:
+		msg.type = MIPI_DSI_DCS_SHORT_WRITE;
+		break;
+
+	case 2:
+		msg.type = MIPI_DSI_DCS_SHORT_WRITE_PARAM;
+		break;
+
+	default:
+		msg.type = MIPI_DSI_DCS_LONG_WRITE;
+		break;
+	}
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_write_buffer);
+
+/**
+ * mipi_dsi_dcs_write() - send DCS write command
+ * @dsi: DSI peripheral device
+ * @cmd: DCS command
+ * @data: buffer containing the command payload
+ * @len: command payload length
+ *
+ * This function will automatically choose the right data type depending on
+ * the command payload length.
+ *
+ * Return: The number of bytes successfully transmitted or a negative error
+ * code on failure.
+ */
+ssize_t mipi_dsi_dcs_write(struct mipi_dsi_device *dsi, u8 cmd,
+			   const void *data, size_t len)
+{
+	ssize_t err;
+	size_t size;
+	u8 *tx;
+
+	if (len > 0) {
+		size = 1 + len;
+
+		tx = kmalloc(size, GFP_KERNEL);
+		if (!tx)
+			return -ENOMEM;
+
+		/* concatenate the DCS command byte and the payload */
+		tx[0] = cmd;
+		memcpy(&tx[1], data, len);
+	} else {
+		tx = &cmd;
+		size = 1;
+	}
+
+	err = mipi_dsi_dcs_write_buffer(dsi, tx, size);
+
+	if (len > 0)
+		kfree(tx);
+
+	return err;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_write);
+
+/**
+ * mipi_dsi_dcs_read() - send DCS read request command
+ * @dsi: DSI peripheral device
+ * @cmd: DCS command
+ * @data: buffer in which to receive data
+ * @len: size of receive buffer
+ *
+ * Return: The number of bytes read or a negative error code on failure.
+ */
+ssize_t mipi_dsi_dcs_read(struct mipi_dsi_device *dsi, u8 cmd, void *data,
+			  size_t len)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.type = MIPI_DSI_DCS_READ,
+		.tx_buf = &cmd,
+		.tx_len = 1,
+		.rx_buf = data,
+		.rx_len = len
+	};
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_read);
+
+/**
+ * mipi_dsi_pixel_format_to_bpp - obtain the number of bits per pixel for any
+ *                                given pixel format defined by the MIPI DSI
+ *                                specification
+ * @fmt: MIPI DSI pixel format
+ *
+ * Returns: The number of bits per pixel of the given pixel format.
+ */
+int mipi_dsi_pixel_format_to_bpp(enum mipi_dsi_pixel_format fmt)
+{
+	switch (fmt) {
+	case MIPI_DSI_FMT_RGB888:
+	case MIPI_DSI_FMT_RGB666:
+		return 24;
+
+	case MIPI_DSI_FMT_RGB666_PACKED:
+		return 18;
+
+	case MIPI_DSI_FMT_RGB565:
+		return 16;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL(mipi_dsi_pixel_format_to_bpp);
+
+/**
+ * mipi_dsi_dcs_nop() - send DCS nop packet
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_nop(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_NOP, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_nop);
+
+/**
+ * mipi_dsi_dcs_soft_reset() - perform a software reset of the display module
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_soft_reset(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SOFT_RESET, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_soft_reset);
+
+/**
+ * mipi_dsi_dcs_get_power_mode() - query the display module's current power
+ *    mode
+ * @dsi: DSI peripheral device
+ * @mode: return location for the current power mode
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_get_power_mode(struct mipi_dsi_device *dsi, u8 *mode)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_read(dsi, MIPI_DCS_GET_POWER_MODE, mode,
+				sizeof(*mode));
+	if (err <= 0) {
+		if (err == 0)
+			err = -ENODATA;
+
+		return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_get_power_mode);
+
+/**
+ * mipi_dsi_dcs_get_pixel_format() - gets the pixel format for the RGB image
+ *    data used by the interface
+ * @dsi: DSI peripheral device
+ * @format: return location for the pixel format
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_get_pixel_format(struct mipi_dsi_device *dsi, u8 *format)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_read(dsi, MIPI_DCS_GET_PIXEL_FORMAT, format,
+				sizeof(*format));
+	if (err <= 0) {
+		if (err == 0)
+			err = -ENODATA;
+
+		return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_get_pixel_format);
+
+/**
+ * mipi_dsi_dcs_enter_sleep_mode() - disable all unnecessary blocks inside the
+ *    display module except interface communication
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_enter_sleep_mode(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_ENTER_SLEEP_MODE, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_enter_sleep_mode);
+
+/**
+ * mipi_dsi_dcs_exit_sleep_mode() - enable all blocks inside the display
+ *    module
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_exit_sleep_mode(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_EXIT_SLEEP_MODE, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_exit_sleep_mode);
+
+/**
+ * mipi_dsi_dcs_set_display_off() - stop displaying the image data on the
+ *    display device
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_display_off(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_DISPLAY_OFF, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_display_off);
+
+/**
+ * mipi_dsi_dcs_set_display_on() - start displaying the image data on the
+ *    display device
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_display_on(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_DISPLAY_ON, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_display_on);
+
+/**
+ * mipi_dsi_dcs_set_column_address() - define the column extent of the frame
+ *    memory accessed by the host processor
+ * @dsi: DSI peripheral device
+ * @start: first column of frame memory
+ * @end: last column of frame memory
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_column_address(struct mipi_dsi_device *dsi, u16 start,
+				    u16 end)
+{
+	u8 payload[4] = { start >> 8, start & 0xff, end >> 8, end & 0xff };
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_COLUMN_ADDRESS, payload,
+				 sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_column_address);
+
+/**
+ * mipi_dsi_dcs_set_page_address() - define the page extent of the frame
+ *    memory accessed by the host processor
+ * @dsi: DSI peripheral device
+ * @start: first page of frame memory
+ * @end: last page of frame memory
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_page_address(struct mipi_dsi_device *dsi, u16 start,
+				  u16 end)
+{
+	u8 payload[4] = { start >> 8, start & 0xff, end >> 8, end & 0xff };
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_PAGE_ADDRESS, payload,
+				 sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_page_address);
+
+/**
+ * mipi_dsi_dcs_set_tear_off() - turn off the display module's Tearing Effect
+ *    output signal on the TE signal line
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_tear_off(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_TEAR_OFF, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_tear_off);
+
+/**
+ * mipi_dsi_dcs_set_tear_on() - turn on the display module's Tearing Effect
+ *    output signal on the TE signal line.
+ * @dsi: DSI peripheral device
+ * @mode: the Tearing Effect Output Line mode
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_tear_on(struct mipi_dsi_device *dsi,
+			     enum mipi_dsi_dcs_tear_mode mode)
+{
+	u8 value = mode;
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_TEAR_ON, &value,
+				 sizeof(value));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_tear_on);
+
+/**
+ * mipi_dsi_dcs_set_pixel_format() - sets the pixel format for the RGB image
+ *    data used by the interface
+ * @dsi: DSI peripheral device
+ * @format: pixel format
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_pixel_format(struct mipi_dsi_device *dsi, u8 format)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_PIXEL_FORMAT, &format,
+				 sizeof(format));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_pixel_format);
+
+/**
+ * mipi_dsi_dcs_set_tear_scanline() - set the scanline to use as trigger for
+ *    the Tearing Effect output signal of the display module
+ * @dsi: DSI peripheral device
+ * @scanline: scanline to use as trigger
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_tear_scanline(struct mipi_dsi_device *dsi, u16 scanline)
+{
+	u8 payload[3] = { MIPI_DCS_SET_TEAR_SCANLINE, scanline >> 8,
+			  scanline & 0xff };
+	ssize_t err;
+
+	err = mipi_dsi_generic_write(dsi, payload, sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_tear_scanline);
+
+/**
+ * mipi_dsi_dcs_set_display_brightness() - sets the brightness value of the
+ *    display
+ * @dsi: DSI peripheral device
+ * @brightness: brightness value
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_display_brightness(struct mipi_dsi_device *dsi,
+					u16 brightness)
+{
+	u8 payload[2] = { brightness & 0xff, brightness >> 8 };
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_DISPLAY_BRIGHTNESS,
+				 payload, sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_display_brightness);
+
+/**
+ * mipi_dsi_dcs_get_display_brightness() - gets the current brightness value
+ *    of the display
+ * @dsi: DSI peripheral device
+ * @brightness: brightness value
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_get_display_brightness(struct mipi_dsi_device *dsi,
+					u16 *brightness)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_read(dsi, MIPI_DCS_GET_DISPLAY_BRIGHTNESS,
+				brightness, sizeof(*brightness));
+	if (err <= 0) {
+		if (err == 0)
+			err = -ENODATA;
+
+		return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_get_display_brightness);
+
+MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
+MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
+MODULE_DESCRIPTION("MIPI DSI Bus");
+MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/video/orisetech_otm8009a.c b/drivers/video/orisetech_otm8009a.c
new file mode 100644
index 0000000..ad1d6f0
--- /dev/null
+++ b/drivers/video/orisetech_otm8009a.c
@@ -0,0 +1,339 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 STMicroelectronics - All Rights Reserved
+ * Author(s): Yannick Fertre <yannick.fertre@st.com> for STMicroelectronics.
+ *            Philippe Cornu <philippe.cornu@st.com> for STMicroelectronics.
+ *
+ * This otm8009a panel driver is based on the Linux Kernel driver from
+ * drivers/gpu/drm/panel/panel-orisetech-otm8009a.c.
+ */
+#include <common.h>
+#include <backlight.h>
+#include <dm.h>
+#include <mipi_display.h>
+#include <panel.h>
+#include <asm/gpio.h>
+#include <power/regulator.h>
+
+#define OTM8009A_BACKLIGHT_DEFAULT	240
+#define OTM8009A_BACKLIGHT_MAX		255
+
+/* Manufacturer Command Set */
+#define MCS_ADRSFT	0x0000	/* Address Shift Function */
+#define MCS_PANSET	0xB3A6	/* Panel Type Setting */
+#define MCS_SD_CTRL	0xC0A2	/* Source Driver Timing Setting */
+#define MCS_P_DRV_M	0xC0B4	/* Panel Driving Mode */
+#define MCS_OSC_ADJ	0xC181	/* Oscillator Adjustment for Idle/Normal mode */
+#define MCS_RGB_VID_SET	0xC1A1	/* RGB Video Mode Setting */
+#define MCS_SD_PCH_CTRL	0xC480	/* Source Driver Precharge Control */
+#define MCS_NO_DOC1	0xC48A	/* Command not documented */
+#define MCS_PWR_CTRL1	0xC580	/* Power Control Setting 1 */
+#define MCS_PWR_CTRL2	0xC590	/* Power Control Setting 2 for Normal Mode */
+#define MCS_PWR_CTRL4	0xC5B0	/* Power Control Setting 4 for DC Voltage */
+#define MCS_PANCTRLSET1	0xCB80	/* Panel Control Setting 1 */
+#define MCS_PANCTRLSET2	0xCB90	/* Panel Control Setting 2 */
+#define MCS_PANCTRLSET3	0xCBA0	/* Panel Control Setting 3 */
+#define MCS_PANCTRLSET4	0xCBB0	/* Panel Control Setting 4 */
+#define MCS_PANCTRLSET5	0xCBC0	/* Panel Control Setting 5 */
+#define MCS_PANCTRLSET6	0xCBD0	/* Panel Control Setting 6 */
+#define MCS_PANCTRLSET7	0xCBE0	/* Panel Control Setting 7 */
+#define MCS_PANCTRLSET8	0xCBF0	/* Panel Control Setting 8 */
+#define MCS_PANU2D1	0xCC80	/* Panel U2D Setting 1 */
+#define MCS_PANU2D2	0xCC90	/* Panel U2D Setting 2 */
+#define MCS_PANU2D3	0xCCA0	/* Panel U2D Setting 3 */
+#define MCS_PAND2U1	0xCCB0	/* Panel D2U Setting 1 */
+#define MCS_PAND2U2	0xCCC0	/* Panel D2U Setting 2 */
+#define MCS_PAND2U3	0xCCD0	/* Panel D2U Setting 3 */
+#define MCS_GOAVST	0xCE80	/* GOA VST Setting */
+#define MCS_GOACLKA1	0xCEA0	/* GOA CLKA1 Setting */
+#define MCS_GOACLKA3	0xCEB0	/* GOA CLKA3 Setting */
+#define MCS_GOAECLK	0xCFC0	/* GOA ECLK Setting */
+#define MCS_NO_DOC2	0xCFD0	/* Command not documented */
+#define MCS_GVDDSET	0xD800	/* GVDD/NGVDD */
+#define MCS_VCOMDC	0xD900	/* VCOM Voltage Setting */
+#define MCS_GMCT2_2P	0xE100	/* Gamma Correction 2.2+ Setting */
+#define MCS_GMCT2_2N	0xE200	/* Gamma Correction 2.2- Setting */
+#define MCS_NO_DOC3	0xF5B6	/* Command not documented */
+#define MCS_CMD2_ENA1	0xFF00	/* Enable Access Command2 "CMD2" */
+#define MCS_CMD2_ENA2	0xFF80	/* Enable Access Orise Command2 */
+
+struct otm8009a_panel_priv {
+	struct udevice *reg;
+	struct gpio_desc reset;
+};
+
+static const struct display_timing default_timing = {
+	.pixelclock = {.min = 32729000, .typ = 32729000, .max = 32729000,},
+	.hactive = {.min = 480, .typ = 480, .max = 480,},
+	.hfront_porch = {.min = 120, .typ = 120, .max = 120,},
+	.hback_porch = {.min = 120, .typ = 120, .max = 120,},
+	.hsync_len = {.min = 63, .typ = 63, .max = 63,},
+	.vactive = {.min = 800, .typ = 800, .max = 800,},
+	.vfront_porch = {.min = 12, .typ = 12, .max = 12,},
+	.vback_porch = {.min = 12, .typ = 12, .max = 12,},
+	.vsync_len = {.min = 12, .typ = 12, .max = 12,},
+};
+
+static void otm8009a_dcs_write_buf(struct udevice *dev, const void *data,
+				   size_t len)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+
+	if (mipi_dsi_dcs_write_buffer(device, data, len) < 0)
+		dev_err(dev, "mipi dsi dcs write buffer failed\n");
+}
+
+#define dcs_write_seq(dev, seq...)				\
+({								\
+	static const u8 d[] = { seq };				\
+	otm8009a_dcs_write_buf(dev, d, ARRAY_SIZE(d));		\
+})
+
+#define dcs_write_cmd_at(dev, cmd, seq...)		\
+({							\
+	static const u16 c = cmd;			\
+	struct udevice *device = dev;			\
+	dcs_write_seq(device, MCS_ADRSFT, (c) & 0xFF);	\
+	dcs_write_seq(device, (c) >> 8, seq);		\
+})
+
+static int otm8009a_init_sequence(struct udevice *dev)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+	int ret;
+
+	/* Enter CMD2 */
+	dcs_write_cmd_at(dev, MCS_CMD2_ENA1, 0x80, 0x09, 0x01);
+
+	/* Enter Orise Command2 */
+	dcs_write_cmd_at(dev, MCS_CMD2_ENA2, 0x80, 0x09);
+
+	dcs_write_cmd_at(dev, MCS_SD_PCH_CTRL, 0x30);
+	mdelay(10);
+
+	dcs_write_cmd_at(dev, MCS_NO_DOC1, 0x40);
+	mdelay(10);
+
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL4 + 1, 0xA9);
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL2 + 1, 0x34);
+	dcs_write_cmd_at(dev, MCS_P_DRV_M, 0x50);
+	dcs_write_cmd_at(dev, MCS_VCOMDC, 0x4E);
+	dcs_write_cmd_at(dev, MCS_OSC_ADJ, 0x66); /* 65Hz */
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL2 + 2, 0x01);
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL2 + 5, 0x34);
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL2 + 4, 0x33);
+	dcs_write_cmd_at(dev, MCS_GVDDSET, 0x79, 0x79);
+	dcs_write_cmd_at(dev, MCS_SD_CTRL + 1, 0x1B);
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL1 + 2, 0x83);
+	dcs_write_cmd_at(dev, MCS_SD_PCH_CTRL + 1, 0x83);
+	dcs_write_cmd_at(dev, MCS_RGB_VID_SET, 0x0E);
+	dcs_write_cmd_at(dev, MCS_PANSET, 0x00, 0x01);
+
+	dcs_write_cmd_at(dev, MCS_GOAVST, 0x85, 0x01, 0x00, 0x84, 0x01, 0x00);
+	dcs_write_cmd_at(dev, MCS_GOACLKA1, 0x18, 0x04, 0x03, 0x39, 0x00, 0x00,
+			 0x00, 0x18, 0x03, 0x03, 0x3A, 0x00, 0x00, 0x00);
+	dcs_write_cmd_at(dev, MCS_GOACLKA3, 0x18, 0x02, 0x03, 0x3B, 0x00, 0x00,
+			 0x00, 0x18, 0x01, 0x03, 0x3C, 0x00, 0x00, 0x00);
+	dcs_write_cmd_at(dev, MCS_GOAECLK, 0x01, 0x01, 0x20, 0x20, 0x00, 0x00,
+			 0x01, 0x02, 0x00, 0x00);
+
+	dcs_write_cmd_at(dev, MCS_NO_DOC2, 0x00);
+
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET5, 0, 4, 4, 4, 4, 4, 0, 0, 0, 0,
+			 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET6, 0, 0, 0, 0, 0, 0, 4, 4, 4, 4,
+			 4, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+	dcs_write_cmd_at(dev, MCS_PANCTRLSET8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+			 0xFF, 0xFF, 0xFF, 0xFF, 0xFF);
+
+	dcs_write_cmd_at(dev, MCS_PANU2D1, 0x00, 0x26, 0x09, 0x0B, 0x01, 0x25,
+			 0x00, 0x00, 0x00, 0x00);
+	dcs_write_cmd_at(dev, MCS_PANU2D2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			 0x00, 0x00, 0x00, 0x00, 0x00, 0x26, 0x0A, 0x0C, 0x02);
+	dcs_write_cmd_at(dev, MCS_PANU2D3, 0x25, 0x00, 0x00, 0x00, 0x00, 0x00,
+			 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
+	dcs_write_cmd_at(dev, MCS_PAND2U1, 0x00, 0x25, 0x0C, 0x0A, 0x02, 0x26,
+			 0x00, 0x00, 0x00, 0x00);
+	dcs_write_cmd_at(dev, MCS_PAND2U2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			 0x00, 0x00, 0x00, 0x00, 0x00, 0x25, 0x0B, 0x09, 0x01);
+	dcs_write_cmd_at(dev, MCS_PAND2U3, 0x26, 0x00, 0x00, 0x00, 0x00, 0x00,
+			 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
+
+	dcs_write_cmd_at(dev, MCS_PWR_CTRL1 + 1, 0x66);
+
+	dcs_write_cmd_at(dev, MCS_NO_DOC3, 0x06);
+
+	dcs_write_cmd_at(dev, MCS_GMCT2_2P, 0x00, 0x09, 0x0F, 0x0E, 0x07, 0x10,
+			 0x0B, 0x0A, 0x04, 0x07, 0x0B, 0x08, 0x0F, 0x10, 0x0A,
+			 0x01);
+	dcs_write_cmd_at(dev, MCS_GMCT2_2N, 0x00, 0x09, 0x0F, 0x0E, 0x07, 0x10,
+			 0x0B, 0x0A, 0x04, 0x07, 0x0B, 0x08, 0x0F, 0x10, 0x0A,
+			 0x01);
+
+	/* Exit CMD2 */
+	dcs_write_cmd_at(dev, MCS_CMD2_ENA1, 0xFF, 0xFF, 0xFF);
+
+	ret =  mipi_dsi_dcs_nop(device);
+	if (ret)
+		return ret;
+
+	ret = mipi_dsi_dcs_exit_sleep_mode(device);
+	if (ret)
+		return ret;
+
+	/* Wait for sleep out exit */
+	mdelay(120);
+
+	/* Default portrait 480x800 rgb24 */
+	dcs_write_seq(dev, MIPI_DCS_SET_ADDRESS_MODE, 0x00);
+
+	ret =  mipi_dsi_dcs_set_column_address(device, 0, 479);
+	if (ret)
+		return ret;
+
+	ret =  mipi_dsi_dcs_set_page_address(device, 0, 799);
+	if (ret)
+		return ret;
+
+	/* See otm8009a driver documentation for pixel format descriptions */
+	ret =  mipi_dsi_dcs_set_pixel_format(device, MIPI_DCS_PIXEL_FMT_24BIT |
+					     MIPI_DCS_PIXEL_FMT_24BIT << 4);
+	if (ret)
+		return ret;
+
+	/* Disable CABC feature */
+	dcs_write_seq(dev, MIPI_DCS_WRITE_POWER_SAVE, 0x00);
+
+	return 0;
+}
+
+static int otm8009a_panel_enable_backlight(struct udevice *dev)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+	int ret;
+
+	device->lanes = 2;
+	device->format = MIPI_DSI_FMT_RGB888;
+	device->mode_flags = MIPI_DSI_MODE_VIDEO |
+			     MIPI_DSI_MODE_VIDEO_BURST |
+			     MIPI_DSI_MODE_LPM;
+
+	ret = mipi_dsi_attach(device);
+	if (ret < 0)
+		return ret;
+
+	ret = otm8009a_init_sequence(dev);
+	if (ret)
+		return ret;
+
+	/*
+	 * Power on the backlight with the requested brightness
+	 * Note We can not use mipi_dsi_dcs_set_display_brightness()
+	 * as otm8009a driver support only 8-bit brightness (1 param).
+	 */
+	dcs_write_seq(dev, MIPI_DCS_SET_DISPLAY_BRIGHTNESS,
+		      OTM8009A_BACKLIGHT_DEFAULT);
+
+	/* Update Brightness Control & Backlight */
+	dcs_write_seq(dev, MIPI_DCS_WRITE_CONTROL_DISPLAY, 0x24);
+
+	ret = mipi_dsi_dcs_set_display_on(device);
+	if (ret)
+		return ret;
+
+	ret = mipi_dsi_dcs_nop(device);
+	if (ret)
+		return ret;
+
+	/* Send Command GRAM memory write (no parameters) */
+	dcs_write_seq(dev, MIPI_DCS_WRITE_MEMORY_START);
+
+	/* need to wait a few time before set the DSI bridge in video mode */
+	mdelay(10);
+
+	return 0;
+}
+
+static int otm8009a_panel_get_display_timing(struct udevice *dev,
+					     struct display_timing *timings)
+{
+	memcpy(timings, &default_timing, sizeof(*timings));
+	return 0;
+}
+
+static int otm8009a_panel_ofdata_to_platdata(struct udevice *dev)
+{
+	struct otm8009a_panel_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	if (IS_ENABLED(CONFIG_DM_REGULATOR)) {
+		ret =  device_get_supply_regulator(dev, "power-supply",
+						   &priv->reg);
+		if (ret && ret != -ENOENT) {
+			dev_err(dev, "Warning: cannot get power supply\n");
+			return ret;
+		}
+	}
+
+	ret = gpio_request_by_name(dev, "reset-gpios", 0, &priv->reset,
+				   GPIOD_IS_OUT);
+	if (ret) {
+		dev_err(dev, "warning: cannot get reset GPIO\n");
+		if (ret != -ENOENT)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int otm8009a_panel_probe(struct udevice *dev)
+{
+	struct otm8009a_panel_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	if (IS_ENABLED(CONFIG_DM_REGULATOR) && priv->reg) {
+		dev_err(dev, "enable regulator '%s'\n", priv->reg->name);
+		ret = regulator_set_enable(priv->reg, true);
+		if (ret)
+			return ret;
+	}
+
+	/* reset panel */
+	dm_gpio_set_value(&priv->reset, true);
+	mdelay(1);
+	dm_gpio_set_value(&priv->reset, false);
+	mdelay(10);
+
+	return 0;
+}
+
+static const struct panel_ops otm8009a_panel_ops = {
+	.enable_backlight = otm8009a_panel_enable_backlight,
+	.get_display_timing = otm8009a_panel_get_display_timing,
+};
+
+static const struct udevice_id otm8009a_panel_ids[] = {
+	{ .compatible = "orisetech,otm8009a" },
+	{ }
+};
+
+U_BOOT_DRIVER(otm8009a_panel) = {
+	.name			  = "otm8009a_panel",
+	.id			  = UCLASS_PANEL,
+	.of_match		  = otm8009a_panel_ids,
+	.ops			  = &otm8009a_panel_ops,
+	.ofdata_to_platdata	  = otm8009a_panel_ofdata_to_platdata,
+	.probe			  = otm8009a_panel_probe,
+	.platdata_auto_alloc_size = sizeof(struct mipi_dsi_panel_plat),
+	.priv_auto_alloc_size	= sizeof(struct otm8009a_panel_priv),
+};
diff --git a/drivers/video/raydium-rm68200.c b/drivers/video/raydium-rm68200.c
new file mode 100644
index 0000000..3347f12
--- /dev/null
+++ b/drivers/video/raydium-rm68200.c
@@ -0,0 +1,338 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 STMicroelectronics - All Rights Reserved
+ * Author(s): Yannick Fertre <yannick.fertre@st.com> for STMicroelectronics.
+ *            Philippe Cornu <philippe.cornu@st.com> for STMicroelectronics.
+ *
+ * This rm68200 panel driver is based on the Linux Kernel driver from
+ * drivers/gpu/drm/panel/panel-raydium-rm68200.c.
+ */
+#include <common.h>
+#include <backlight.h>
+#include <dm.h>
+#include <mipi_display.h>
+#include <panel.h>
+#include <asm/gpio.h>
+#include <power/regulator.h>
+
+/*** Manufacturer Command Set ***/
+#define MCS_CMD_MODE_SW	0xFE /* CMD Mode Switch */
+#define MCS_CMD1_UCS	0x00 /* User Command Set (UCS = CMD1) */
+#define MCS_CMD2_P0	0x01 /* Manufacture Command Set Page0 (CMD2 P0) */
+#define MCS_CMD2_P1	0x02 /* Manufacture Command Set Page1 (CMD2 P1) */
+#define MCS_CMD2_P2	0x03 /* Manufacture Command Set Page2 (CMD2 P2) */
+#define MCS_CMD2_P3	0x04 /* Manufacture Command Set Page3 (CMD2 P3) */
+
+/* CMD2 P0 commands (Display Options and Power) */
+#define MCS_STBCTR	0x12 /* TE1 Output Setting Zig-Zag Connection */
+#define MCS_SGOPCTR	0x16 /* Source Bias Current */
+#define MCS_SDCTR	0x1A /* Source Output Delay Time */
+#define MCS_INVCTR	0x1B /* Inversion Type */
+#define MCS_EXT_PWR_IC	0x24 /* External PWR IC Control */
+#define MCS_SETAVDD	0x27 /* PFM Control for AVDD Output */
+#define MCS_SETAVEE	0x29 /* PFM Control for AVEE Output */
+#define MCS_BT2CTR	0x2B /* DDVDL Charge Pump Control */
+#define MCS_BT3CTR	0x2F /* VGH Charge Pump Control */
+#define MCS_BT4CTR	0x34 /* VGL Charge Pump Control */
+#define MCS_VCMCTR	0x46 /* VCOM Output Level Control */
+#define MCS_SETVGN	0x52 /* VG M/S N Control */
+#define MCS_SETVGP	0x54 /* VG M/S P Control */
+#define MCS_SW_CTRL	0x5F /* Interface Control for PFM and MIPI */
+
+/* CMD2 P2 commands (GOA Timing Control) - no description in datasheet */
+#define GOA_VSTV1		0x00
+#define GOA_VSTV2		0x07
+#define GOA_VCLK1		0x0E
+#define GOA_VCLK2		0x17
+#define GOA_VCLK_OPT1		0x20
+#define GOA_BICLK1		0x2A
+#define GOA_BICLK2		0x37
+#define GOA_BICLK3		0x44
+#define GOA_BICLK4		0x4F
+#define GOA_BICLK_OPT1		0x5B
+#define GOA_BICLK_OPT2		0x60
+#define MCS_GOA_GPO1		0x6D
+#define MCS_GOA_GPO2		0x71
+#define MCS_GOA_EQ		0x74
+#define MCS_GOA_CLK_GALLON	0x7C
+#define MCS_GOA_FS_SEL0		0x7E
+#define MCS_GOA_FS_SEL1		0x87
+#define MCS_GOA_FS_SEL2		0x91
+#define MCS_GOA_FS_SEL3		0x9B
+#define MCS_GOA_BS_SEL0		0xAC
+#define MCS_GOA_BS_SEL1		0xB5
+#define MCS_GOA_BS_SEL2		0xBF
+#define MCS_GOA_BS_SEL3		0xC9
+#define MCS_GOA_BS_SEL4		0xD3
+
+/* CMD2 P3 commands (Gamma) */
+#define MCS_GAMMA_VP		0x60 /* Gamma VP1~VP16 */
+#define MCS_GAMMA_VN		0x70 /* Gamma VN1~VN16 */
+
+struct rm68200_panel_priv {
+	struct udevice *reg;
+	struct udevice *backlight;
+	struct gpio_desc reset;
+};
+
+static const struct display_timing default_timing = {
+	.pixelclock = {.min = 52582000, .typ = 52582000, .max = 52582000,},
+	.hactive = {.min = 720, .typ = 720, .max = 720,},
+	.hfront_porch = {.min = 38, .typ = 38, .max = 38,},
+	.hback_porch = {.min = 8, .typ = 8, .max = 8,},
+	.hsync_len = {.min = 38, .typ = 38, .max = 38,},
+	.vactive = {.min = 1280, .typ = 1280, .max = 1280,},
+	.vfront_porch = {.min = 12, .typ = 12, .max = 12,},
+	.vback_porch = {.min = 4, .typ = 4, .max = 4,},
+	.vsync_len = {.min = 12, .typ = 12, .max = 12,},
+};
+
+static void rm68200_dcs_write_buf(struct udevice *dev, const void *data,
+				  size_t len)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+	int err;
+
+	err = mipi_dsi_dcs_write_buffer(device, data, len);
+	if (err < 0)
+		dev_err(dev, "MIPI DSI DCS write buffer failed: %d\n", err);
+}
+
+static void rm68200_dcs_write_cmd(struct udevice *dev, u8 cmd, u8 value)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+	int err;
+
+	err = mipi_dsi_dcs_write(device, cmd, &value, 1);
+	if (err < 0)
+		dev_err(dev, "MIPI DSI DCS write failed: %d\n", err);
+}
+
+#define dcs_write_seq(ctx, seq...)				\
+({								\
+	static const u8 d[] = { seq };				\
+								\
+	rm68200_dcs_write_buf(ctx, d, ARRAY_SIZE(d));		\
+})
+
+/*
+ * This panel is not able to auto-increment all cmd addresses so for some of
+ * them, we need to send them one by one...
+ */
+#define dcs_write_cmd_seq(ctx, cmd, seq...)			\
+({								\
+	static const u8 d[] = { seq };				\
+	unsigned int i;						\
+								\
+	for (i = 0; i < ARRAY_SIZE(d) ; i++)			\
+		rm68200_dcs_write_cmd(ctx, cmd + i, d[i]);	\
+})
+
+static void rm68200_init_sequence(struct udevice *dev)
+{
+	/* Enter CMD2 with page 0 */
+	dcs_write_seq(dev, MCS_CMD_MODE_SW, MCS_CMD2_P0);
+	dcs_write_cmd_seq(dev, MCS_EXT_PWR_IC, 0xC0, 0x53, 0x00);
+	dcs_write_seq(dev, MCS_BT2CTR, 0xE5);
+	dcs_write_seq(dev, MCS_SETAVDD, 0x0A);
+	dcs_write_seq(dev, MCS_SETAVEE, 0x0A);
+	dcs_write_seq(dev, MCS_SGOPCTR, 0x52);
+	dcs_write_seq(dev, MCS_BT3CTR, 0x53);
+	dcs_write_seq(dev, MCS_BT4CTR, 0x5A);
+	dcs_write_seq(dev, MCS_INVCTR, 0x00);
+	dcs_write_seq(dev, MCS_STBCTR, 0x0A);
+	dcs_write_seq(dev, MCS_SDCTR, 0x06);
+	dcs_write_seq(dev, MCS_VCMCTR, 0x56);
+	dcs_write_seq(dev, MCS_SETVGN, 0xA0, 0x00);
+	dcs_write_seq(dev, MCS_SETVGP, 0xA0, 0x00);
+	dcs_write_seq(dev, MCS_SW_CTRL, 0x11); /* 2 data lanes, see doc */
+
+	dcs_write_seq(dev, MCS_CMD_MODE_SW, MCS_CMD2_P2);
+	dcs_write_seq(dev, GOA_VSTV1, 0x05);
+	dcs_write_seq(dev, 0x02, 0x0B);
+	dcs_write_seq(dev, 0x03, 0x0F);
+	dcs_write_seq(dev, 0x04, 0x7D, 0x00, 0x50);
+	dcs_write_cmd_seq(dev, GOA_VSTV2, 0x05, 0x16, 0x0D, 0x11, 0x7D, 0x00,
+			  0x50);
+	dcs_write_cmd_seq(dev, GOA_VCLK1, 0x07, 0x08, 0x01, 0x02, 0x00, 0x7D,
+			  0x00, 0x85, 0x08);
+	dcs_write_cmd_seq(dev, GOA_VCLK2, 0x03, 0x04, 0x05, 0x06, 0x00, 0x7D,
+			  0x00, 0x85, 0x08);
+	dcs_write_seq(dev, GOA_VCLK_OPT1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		      0x00, 0x00, 0x00, 0x00);
+	dcs_write_cmd_seq(dev, GOA_BICLK1, 0x07, 0x08);
+	dcs_write_seq(dev, 0x2D, 0x01);
+	dcs_write_seq(dev, 0x2F, 0x02, 0x00, 0x40, 0x05, 0x08, 0x54, 0x7D,
+		      0x00);
+	dcs_write_cmd_seq(dev, GOA_BICLK2, 0x03, 0x04, 0x05, 0x06, 0x00);
+	dcs_write_seq(dev, 0x3D, 0x40);
+	dcs_write_seq(dev, 0x3F, 0x05, 0x08, 0x54, 0x7D, 0x00);
+	dcs_write_seq(dev, GOA_BICLK3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		      0x00, 0x00, 0x00, 0x00, 0x00);
+	dcs_write_seq(dev, GOA_BICLK4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		      0x00, 0x00);
+	dcs_write_seq(dev, 0x58, 0x00, 0x00, 0x00);
+	dcs_write_seq(dev, GOA_BICLK_OPT1, 0x00, 0x00, 0x00, 0x00, 0x00);
+	dcs_write_seq(dev, GOA_BICLK_OPT2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
+	dcs_write_seq(dev, MCS_GOA_GPO1, 0x00, 0x00, 0x00, 0x00);
+	dcs_write_seq(dev, MCS_GOA_GPO2, 0x00, 0x20, 0x00);
+	dcs_write_seq(dev, MCS_GOA_EQ, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		      0x00, 0x00);
+	dcs_write_seq(dev, MCS_GOA_CLK_GALLON, 0x00, 0x00);
+	dcs_write_cmd_seq(dev, MCS_GOA_FS_SEL0, 0xBF, 0x02, 0x06, 0x14, 0x10,
+			  0x16, 0x12, 0x08, 0x3F);
+	dcs_write_cmd_seq(dev, MCS_GOA_FS_SEL1, 0x3F, 0x3F, 0x3F, 0x3F, 0x0C,
+			  0x0A, 0x0E, 0x3F, 0x3F, 0x00);
+	dcs_write_cmd_seq(dev, MCS_GOA_FS_SEL2, 0x04, 0x3F, 0x3F, 0x3F, 0x3F,
+			  0x05, 0x01, 0x3F, 0x3F, 0x0F);
+	dcs_write_cmd_seq(dev, MCS_GOA_FS_SEL3, 0x0B, 0x0D, 0x3F, 0x3F, 0x3F,
+			  0x3F);
+	dcs_write_cmd_seq(dev, 0xA2, 0x3F, 0x09, 0x13, 0x17, 0x11, 0x15);
+	dcs_write_cmd_seq(dev, 0xA9, 0x07, 0x03, 0x3F);
+	dcs_write_cmd_seq(dev, MCS_GOA_BS_SEL0, 0x3F, 0x05, 0x01, 0x17, 0x13,
+			  0x15, 0x11, 0x0F, 0x3F);
+	dcs_write_cmd_seq(dev, MCS_GOA_BS_SEL1, 0x3F, 0x3F, 0x3F, 0x3F, 0x0B,
+			  0x0D, 0x09, 0x3F, 0x3F, 0x07);
+	dcs_write_cmd_seq(dev, MCS_GOA_BS_SEL2, 0x03, 0x3F, 0x3F, 0x3F, 0x3F,
+			  0x02, 0x06, 0x3F, 0x3F, 0x08);
+	dcs_write_cmd_seq(dev, MCS_GOA_BS_SEL3, 0x0C, 0x0A, 0x3F, 0x3F, 0x3F,
+			  0x3F, 0x3F, 0x0E, 0x10, 0x14);
+	dcs_write_cmd_seq(dev, MCS_GOA_BS_SEL4, 0x12, 0x16, 0x00, 0x04, 0x3F);
+	dcs_write_seq(dev, 0xDC, 0x02);
+	dcs_write_seq(dev, 0xDE, 0x12);
+
+	dcs_write_seq(dev, MCS_CMD_MODE_SW, 0x0E); /* No documentation */
+	dcs_write_seq(dev, 0x01, 0x75);
+
+	dcs_write_seq(dev, MCS_CMD_MODE_SW, MCS_CMD2_P3);
+	dcs_write_cmd_seq(dev, MCS_GAMMA_VP, 0x00, 0x0C, 0x12, 0x0E, 0x06,
+			  0x12, 0x0E, 0x0B, 0x15, 0x0B, 0x10, 0x07, 0x0F,
+			  0x12, 0x0C, 0x00);
+	dcs_write_cmd_seq(dev, MCS_GAMMA_VN, 0x00, 0x0C, 0x12, 0x0E, 0x06,
+			  0x12, 0x0E, 0x0B, 0x15, 0x0B, 0x10, 0x07, 0x0F,
+			  0x12, 0x0C, 0x00);
+
+	/* Exit CMD2 */
+	dcs_write_seq(dev, MCS_CMD_MODE_SW, MCS_CMD1_UCS);
+}
+
+static int rm68200_panel_enable_backlight(struct udevice *dev)
+{
+	struct mipi_dsi_panel_plat *plat = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = plat->device;
+	struct rm68200_panel_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	device->lanes = 2;
+	device->format = MIPI_DSI_FMT_RGB888;
+	device->mode_flags = MIPI_DSI_MODE_VIDEO |
+			     MIPI_DSI_MODE_VIDEO_BURST |
+			     MIPI_DSI_MODE_LPM;
+
+	ret = mipi_dsi_attach(device);
+	if (ret < 0)
+		return ret;
+
+	rm68200_init_sequence(dev);
+
+	ret = mipi_dsi_dcs_exit_sleep_mode(device);
+	if (ret)
+		return ret;
+
+	mdelay(125);
+
+	ret = mipi_dsi_dcs_set_display_on(device);
+	if (ret)
+		return ret;
+
+	mdelay(20);
+
+	ret = backlight_enable(priv->backlight);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int rm68200_panel_get_display_timing(struct udevice *dev,
+					    struct display_timing *timings)
+{
+	memcpy(timings, &default_timing, sizeof(*timings));
+	return 0;
+}
+
+static int rm68200_panel_ofdata_to_platdata(struct udevice *dev)
+{
+	struct rm68200_panel_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	if (IS_ENABLED(CONFIG_DM_REGULATOR)) {
+		ret =  device_get_supply_regulator(dev, "power-supply",
+						   &priv->reg);
+		if (ret && ret != -ENOENT) {
+			dev_err(dev, "Warning: cannot get power supply\n");
+			return ret;
+		}
+	}
+
+	ret = gpio_request_by_name(dev, "reset-gpios", 0, &priv->reset,
+				   GPIOD_IS_OUT);
+	if (ret) {
+		dev_err(dev, "Warning: cannot get reset GPIO\n");
+		if (ret != -ENOENT)
+			return ret;
+	}
+
+	ret = uclass_get_device_by_phandle(UCLASS_PANEL_BACKLIGHT, dev,
+					   "backlight", &priv->backlight);
+	if (ret) {
+		dev_err(dev, "Cannot get backlight: ret=%d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int rm68200_panel_probe(struct udevice *dev)
+{
+	struct rm68200_panel_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	if (IS_ENABLED(CONFIG_DM_REGULATOR) && priv->reg) {
+		ret = regulator_set_enable(priv->reg, true);
+		if (ret)
+			return ret;
+	}
+
+	/* reset panel */
+	dm_gpio_set_value(&priv->reset, true);
+	mdelay(1);
+	dm_gpio_set_value(&priv->reset, false);
+	mdelay(10);
+
+	return 0;
+}
+
+static const struct panel_ops rm68200_panel_ops = {
+	.enable_backlight = rm68200_panel_enable_backlight,
+	.get_display_timing = rm68200_panel_get_display_timing,
+};
+
+static const struct udevice_id rm68200_panel_ids[] = {
+	{ .compatible = "raydium,rm68200" },
+	{ }
+};
+
+U_BOOT_DRIVER(rm68200_panel) = {
+	.name			  = "rm68200_panel",
+	.id			  = UCLASS_PANEL,
+	.of_match		  = rm68200_panel_ids,
+	.ops			  = &rm68200_panel_ops,
+	.ofdata_to_platdata	  = rm68200_panel_ofdata_to_platdata,
+	.probe			  = rm68200_panel_probe,
+	.platdata_auto_alloc_size = sizeof(struct mipi_dsi_panel_plat),
+	.priv_auto_alloc_size	= sizeof(struct rm68200_panel_priv),
+};
diff --git a/drivers/video/stm32/Kconfig b/drivers/video/stm32/Kconfig
index 78b1fac..95d51bb 100644
--- a/drivers/video/stm32/Kconfig
+++ b/drivers/video/stm32/Kconfig
@@ -13,6 +13,15 @@ menuconfig VIDEO_STM32
 	  DSI. This option enables these supports which can be used on
 	  devices which have RGB TFT or DSI display connected.
 
+config VIDEO_STM32_DSI
+	bool "Enable STM32 DSI video support"
+	depends on VIDEO_STM32
+	select VIDEO_BRIDGE
+	select VIDEO_DW_MIPI_DSI
+	help
+	  This option enables support DSI internal bridge which can be used on
+	  devices which have DSI devices connected.
+
 config VIDEO_STM32_MAX_XRES
 	int "Maximum horizontal resolution (for memory allocation purposes)"
 	depends on VIDEO_STM32
diff --git a/drivers/video/stm32/Makefile b/drivers/video/stm32/Makefile
index 7297e5f..f8b42d1 100644
--- a/drivers/video/stm32/Makefile
+++ b/drivers/video/stm32/Makefile
@@ -6,3 +6,4 @@
 #          Yannick Fertre <yannick.fertre@st.com>
 
 obj-${CONFIG_VIDEO_STM32} = stm32_ltdc.o
+obj-${CONFIG_VIDEO_STM32_DSI} += stm32_dsi.o
diff --git a/drivers/video/stm32/stm32_dsi.c b/drivers/video/stm32/stm32_dsi.c
new file mode 100644
index 0000000..09266fe
--- /dev/null
+++ b/drivers/video/stm32/stm32_dsi.c
@@ -0,0 +1,446 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018 STMicroelectronics - All Rights Reserved
+ * Author(s): Philippe Cornu <philippe.cornu@st.com> for STMicroelectronics.
+ *	      Yannick Fertre <yannick.fertre@st.com> for STMicroelectronics.
+ *
+ * This MIPI DSI controller driver is based on the Linux Kernel driver from
+ * drivers/gpu/drm/stm/dw_mipi_dsi-stm.c.
+ */
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <dw_mipi_dsi.h>
+#include <mipi_display.h>
+#include <panel.h>
+#include <reset.h>
+#include <video.h>
+#include <video_bridge.h>
+#include <asm/io.h>
+#include <asm/arch/gpio.h>
+#include <dm/device-internal.h>
+#include <linux/iopoll.h>
+#include <power/regulator.h>
+
+#define HWVER_130			0x31333000	/* IP version 1.30 */
+#define HWVER_131			0x31333100	/* IP version 1.31 */
+
+/* DSI digital registers & bit definitions */
+#define DSI_VERSION			0x00
+#define VERSION				GENMASK(31, 8)
+
+/*
+ * DSI wrapper registers & bit definitions
+ * Note: registers are named as in the Reference Manual
+ */
+#define DSI_WCFGR	0x0400		/* Wrapper ConFiGuration Reg */
+#define WCFGR_DSIM	BIT(0)		/* DSI Mode */
+#define WCFGR_COLMUX	GENMASK(3, 1)	/* COLor MUltipleXing */
+
+#define DSI_WCR		0x0404		/* Wrapper Control Reg */
+#define WCR_DSIEN	BIT(3)		/* DSI ENable */
+
+#define DSI_WISR	0x040C		/* Wrapper Interrupt and Status Reg */
+#define WISR_PLLLS	BIT(8)		/* PLL Lock Status */
+#define WISR_RRS	BIT(12)		/* Regulator Ready Status */
+
+#define DSI_WPCR0	0x0418		/* Wrapper Phy Conf Reg 0 */
+#define WPCR0_UIX4	GENMASK(5, 0)	/* Unit Interval X 4 */
+#define WPCR0_TDDL	BIT(16)		/* Turn Disable Data Lanes */
+
+#define DSI_WRPCR	0x0430		/* Wrapper Regulator & Pll Ctrl Reg */
+#define WRPCR_PLLEN	BIT(0)		/* PLL ENable */
+#define WRPCR_NDIV	GENMASK(8, 2)	/* pll loop DIVision Factor */
+#define WRPCR_IDF	GENMASK(14, 11)	/* pll Input Division Factor */
+#define WRPCR_ODF	GENMASK(17, 16)	/* pll Output Division Factor */
+#define WRPCR_REGEN	BIT(24)		/* REGulator ENable */
+#define WRPCR_BGREN	BIT(28)		/* BandGap Reference ENable */
+#define IDF_MIN		1
+#define IDF_MAX		7
+#define NDIV_MIN	10
+#define NDIV_MAX	125
+#define ODF_MIN		1
+#define ODF_MAX		8
+
+/* dsi color format coding according to the datasheet */
+enum dsi_color {
+	DSI_RGB565_CONF1,
+	DSI_RGB565_CONF2,
+	DSI_RGB565_CONF3,
+	DSI_RGB666_CONF1,
+	DSI_RGB666_CONF2,
+	DSI_RGB888,
+};
+
+#define LANE_MIN_KBPS	31250
+#define LANE_MAX_KBPS	500000
+
+/* Timeout for regulator on/off, pll lock/unlock & fifo empty */
+#define TIMEOUT_US	200000
+
+struct stm32_dsi_priv {
+	struct mipi_dsi_device device;
+	void __iomem *base;
+	struct udevice *panel;
+	u32 pllref_clk;
+	u32 hw_version;
+	int lane_min_kbps;
+	int lane_max_kbps;
+	struct udevice *vdd_reg;
+};
+
+static inline void dsi_write(struct stm32_dsi_priv *dsi, u32 reg, u32 val)
+{
+	writel(val, dsi->base + reg);
+}
+
+static inline u32 dsi_read(struct stm32_dsi_priv *dsi, u32 reg)
+{
+	return readl(dsi->base + reg);
+}
+
+static inline void dsi_set(struct stm32_dsi_priv *dsi, u32 reg, u32 mask)
+{
+	dsi_write(dsi, reg, dsi_read(dsi, reg) | mask);
+}
+
+static inline void dsi_clear(struct stm32_dsi_priv *dsi, u32 reg, u32 mask)
+{
+	dsi_write(dsi, reg, dsi_read(dsi, reg) & ~mask);
+}
+
+static inline void dsi_update_bits(struct stm32_dsi_priv *dsi, u32 reg,
+				   u32 mask, u32 val)
+{
+	dsi_write(dsi, reg, (dsi_read(dsi, reg) & ~mask) | val);
+}
+
+static enum dsi_color dsi_color_from_mipi(u32 fmt)
+{
+	switch (fmt) {
+	case MIPI_DSI_FMT_RGB888:
+		return DSI_RGB888;
+	case MIPI_DSI_FMT_RGB666:
+		return DSI_RGB666_CONF2;
+	case MIPI_DSI_FMT_RGB666_PACKED:
+		return DSI_RGB666_CONF1;
+	case MIPI_DSI_FMT_RGB565:
+		return DSI_RGB565_CONF1;
+	default:
+		pr_err("MIPI color invalid, so we use rgb888\n");
+	}
+	return DSI_RGB888;
+}
+
+static int dsi_pll_get_clkout_khz(int clkin_khz, int idf, int ndiv, int odf)
+{
+	int divisor = idf * odf;
+
+	/* prevent from division by 0 */
+	if (!divisor)
+		return 0;
+
+	return DIV_ROUND_CLOSEST(clkin_khz * ndiv, divisor);
+}
+
+static int dsi_pll_get_params(struct stm32_dsi_priv *dsi,
+			      int clkin_khz, int clkout_khz,
+			      int *idf, int *ndiv, int *odf)
+{
+	int i, o, n, n_min, n_max;
+	int fvco_min, fvco_max, delta, best_delta; /* all in khz */
+
+	/* Early checks preventing division by 0 & odd results */
+	if (clkin_khz <= 0 || clkout_khz <= 0)
+		return -EINVAL;
+
+	fvco_min = dsi->lane_min_kbps * 2 * ODF_MAX;
+	fvco_max = dsi->lane_max_kbps * 2 * ODF_MIN;
+
+	best_delta = 1000000; /* big started value (1000000khz) */
+
+	for (i = IDF_MIN; i <= IDF_MAX; i++) {
+		/* Compute ndiv range according to Fvco */
+		n_min = ((fvco_min * i) / (2 * clkin_khz)) + 1;
+		n_max = (fvco_max * i) / (2 * clkin_khz);
+
+		/* No need to continue idf loop if we reach ndiv max */
+		if (n_min >= NDIV_MAX)
+			break;
+
+		/* Clamp ndiv to valid values */
+		if (n_min < NDIV_MIN)
+			n_min = NDIV_MIN;
+		if (n_max > NDIV_MAX)
+			n_max = NDIV_MAX;
+
+		for (o = ODF_MIN; o <= ODF_MAX; o *= 2) {
+			n = DIV_ROUND_CLOSEST(i * o * clkout_khz, clkin_khz);
+			/* Check ndiv according to vco range */
+			if (n < n_min || n > n_max)
+				continue;
+			/* Check if new delta is better & saves parameters */
+			delta = dsi_pll_get_clkout_khz(clkin_khz, i, n, o) -
+				clkout_khz;
+			if (delta < 0)
+				delta = -delta;
+			if (delta < best_delta) {
+				*idf = i;
+				*ndiv = n;
+				*odf = o;
+				best_delta = delta;
+			}
+			/* fast return in case of "perfect result" */
+			if (!delta)
+				return 0;
+		}
+	}
+
+	return 0;
+}
+
+static int dsi_phy_init(void *priv_data)
+{
+	struct mipi_dsi_device *device = priv_data;
+	struct udevice *dev = device->dev;
+	struct stm32_dsi_priv *dsi = dev_get_priv(dev);
+	u32 val;
+	int ret;
+
+	/* Enable the regulator */
+	dsi_set(dsi, DSI_WRPCR, WRPCR_REGEN | WRPCR_BGREN);
+	ret = readl_poll_timeout(dsi->base + DSI_WISR, val, val & WISR_RRS,
+				 TIMEOUT_US);
+	if (ret) {
+		dev_err(dev, "!TIMEOUT! waiting REGU\n");
+		return ret;
+	}
+
+	/* Enable the DSI PLL & wait for its lock */
+	dsi_set(dsi, DSI_WRPCR, WRPCR_PLLEN);
+	ret = readl_poll_timeout(dsi->base + DSI_WISR, val, val & WISR_PLLLS,
+				 TIMEOUT_US);
+	if (ret) {
+		dev_err(dev, "!TIMEOUT! waiting PLL\n");
+		return ret;
+	}
+
+	/* Enable the DSI wrapper */
+	dsi_set(dsi, DSI_WCR, WCR_DSIEN);
+
+	return 0;
+}
+
+static int dsi_get_lane_mbps(void *priv_data, struct display_timing *timings,
+			     u32 lanes, u32 format, unsigned int *lane_mbps)
+{
+	struct mipi_dsi_device *device = priv_data;
+	struct udevice *dev = device->dev;
+	struct stm32_dsi_priv *dsi = dev_get_priv(dev);
+	int idf, ndiv, odf, pll_in_khz, pll_out_khz;
+	int ret, bpp;
+	u32 val;
+
+	/* Update lane capabilities according to hw version */
+	dsi->hw_version = dsi_read(dsi, DSI_VERSION) & VERSION;
+	dsi->lane_min_kbps = LANE_MIN_KBPS;
+	dsi->lane_max_kbps = LANE_MAX_KBPS;
+	if (dsi->hw_version == HWVER_131) {
+		dsi->lane_min_kbps *= 2;
+		dsi->lane_max_kbps *= 2;
+	}
+
+	pll_in_khz = dsi->pllref_clk / 1000;
+
+	/* Compute requested pll out */
+	bpp = mipi_dsi_pixel_format_to_bpp(format);
+	pll_out_khz = (timings->pixelclock.typ / 1000) * bpp / lanes;
+	/* Add 20% to pll out to be higher than pixel bw (burst mode only) */
+	pll_out_khz = (pll_out_khz * 12) / 10;
+	if (pll_out_khz > dsi->lane_max_kbps) {
+		pll_out_khz = dsi->lane_max_kbps;
+		dev_warn(dev, "Warning max phy mbps is used\n");
+	}
+	if (pll_out_khz < dsi->lane_min_kbps) {
+		pll_out_khz = dsi->lane_min_kbps;
+		dev_warn(dev, "Warning min phy mbps is used\n");
+	}
+
+	/* Compute best pll parameters */
+	idf = 0;
+	ndiv = 0;
+	odf = 0;
+	ret = dsi_pll_get_params(dsi, pll_in_khz, pll_out_khz,
+				 &idf, &ndiv, &odf);
+	if (ret) {
+		dev_err(dev, "Warning dsi_pll_get_params(): bad params\n");
+		return ret;
+	}
+
+	/* Get the adjusted pll out value */
+	pll_out_khz = dsi_pll_get_clkout_khz(pll_in_khz, idf, ndiv, odf);
+
+	/* Set the PLL division factors */
+	dsi_update_bits(dsi, DSI_WRPCR,	WRPCR_NDIV | WRPCR_IDF | WRPCR_ODF,
+			(ndiv << 2) | (idf << 11) | ((ffs(odf) - 1) << 16));
+
+	/* Compute uix4 & set the bit period in high-speed mode */
+	val = 4000000 / pll_out_khz;
+	dsi_update_bits(dsi, DSI_WPCR0, WPCR0_UIX4, val);
+
+	/* Select video mode by resetting DSIM bit */
+	dsi_clear(dsi, DSI_WCFGR, WCFGR_DSIM);
+
+	/* Select the color coding */
+	dsi_update_bits(dsi, DSI_WCFGR, WCFGR_COLMUX,
+			dsi_color_from_mipi(format) << 1);
+
+	*lane_mbps = pll_out_khz / 1000;
+
+	debug("pll_in %ukHz pll_out %ukHz lane_mbps %uMHz\n",
+	      pll_in_khz, pll_out_khz, *lane_mbps);
+
+	return 0;
+}
+
+static const struct dw_mipi_dsi_phy_ops dw_mipi_dsi_stm_phy_ops = {
+	.init = dsi_phy_init,
+	.get_lane_mbps = dsi_get_lane_mbps,
+};
+
+static int stm32_dsi_attach(struct udevice *dev)
+{
+	struct stm32_dsi_priv *priv = dev_get_priv(dev);
+	struct dw_mipi_dsi_plat_data *platdata = dev_get_platdata(dev);
+	struct mipi_dsi_device *device = &priv->device;
+	int ret;
+
+	platdata->max_data_lanes = 2;
+	platdata->phy_ops = &dw_mipi_dsi_stm_phy_ops;
+
+	ret = uclass_first_device(UCLASS_PANEL, &platdata->panel);
+	if (ret) {
+		dev_err(dev, "panel device error %d\n", ret);
+		return ret;
+	}
+
+	ret = dw_mipi_dsi_init_bridge(device);
+	if (ret) {
+		dev_err(dev, "failed to initialize mipi dsi host\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_dsi_set_backlight(struct udevice *dev, int percent)
+{
+	struct dw_mipi_dsi_plat_data *dplat = dev_get_platdata(dev);
+	struct stm32_dsi_priv *priv = dev_get_priv(dev);
+	struct mipi_dsi_device *device = &priv->device;
+	struct udevice *panel = dplat->panel;
+	struct mipi_dsi_panel_plat *mplat;
+	int ret;
+
+	mplat = dev_get_platdata(panel);
+	mplat->device = device;
+
+	ret = panel_enable_backlight(panel);
+	if (ret) {
+		dev_err(dev, "panel %s enable backlight error %d\n",
+			panel->name, ret);
+		return ret;
+	}
+
+	dw_mipi_dsi_bridge_enable(device);
+
+	return 0;
+}
+
+static int stm32_dsi_probe(struct udevice *dev)
+{
+	struct stm32_dsi_priv *priv = dev_get_priv(dev);
+	struct mipi_dsi_device *device = &priv->device;
+	struct reset_ctl rst;
+	struct clk clk;
+	int ret;
+
+	device->dev = dev;
+
+	priv->base = (void *)dev_read_addr(dev);
+	if ((fdt_addr_t)priv->base == FDT_ADDR_T_NONE) {
+		dev_err(dev, "dsi dt register address error\n");
+		return -EINVAL;
+	}
+
+	if (IS_ENABLED(CONFIG_DM_REGULATOR)) {
+		ret =  device_get_supply_regulator(dev, "phy-dsi-supply",
+						   &priv->vdd_reg);
+		if (ret && ret != -ENOENT) {
+			dev_err(dev, "Warning: cannot get phy dsi supply\n");
+			return -ENODEV;
+		}
+
+		ret = regulator_set_enable(priv->vdd_reg, true);
+		if (ret)
+			return -ENODEV;
+	}
+
+	ret = clk_get_by_name(device->dev, "pclk", &clk);
+	if (ret) {
+		dev_err(dev, "peripheral clock get error %d\n", ret);
+		regulator_set_enable(priv->vdd_reg, false);
+		return -ENODEV;
+	}
+
+	ret = clk_enable(&clk);
+	if (ret) {
+		dev_err(dev, "peripheral clock enable error %d\n", ret);
+		regulator_set_enable(priv->vdd_reg, false);
+		return -ENODEV;
+	}
+
+	ret = clk_get_by_name(dev, "ref", &clk);
+	if (ret) {
+		dev_err(dev, "pll reference clock get error %d\n", ret);
+		clk_disable(&clk);
+		regulator_set_enable(priv->vdd_reg, false);
+		return ret;
+	}
+
+	priv->pllref_clk = (unsigned int)clk_get_rate(&clk);
+
+	ret = reset_get_by_index(device->dev, 0, &rst);
+	if (ret) {
+		dev_err(dev, "missing dsi hardware reset\n");
+		clk_disable(&clk);
+		regulator_set_enable(priv->vdd_reg, false);
+		return -ENODEV;
+	}
+
+	/* Reset */
+	reset_deassert(&rst);
+
+	return 0;
+}
+
+struct video_bridge_ops stm32_dsi_ops = {
+	.attach = stm32_dsi_attach,
+	.set_backlight = stm32_dsi_set_backlight,
+};
+
+static const struct udevice_id stm32_dsi_ids[] = {
+	{ .compatible = "st,stm32-dsi"},
+	{ }
+};
+
+U_BOOT_DRIVER(stm32_dsi) = {
+	.name				= "stm32-display-dsi",
+	.id				= UCLASS_VIDEO_BRIDGE,
+	.of_match			= stm32_dsi_ids,
+	.bind				= dm_scan_fdt_dev,
+	.probe				= stm32_dsi_probe,
+	.ops				= &stm32_dsi_ops,
+	.priv_auto_alloc_size		= sizeof(struct stm32_dsi_priv),
+	.platdata_auto_alloc_size	= sizeof(struct dw_mipi_dsi_plat_data),
+};
diff --git a/drivers/video/stm32/stm32_ltdc.c b/drivers/video/stm32/stm32_ltdc.c
index dc6c889..8c996b8 100644
--- a/drivers/video/stm32/stm32_ltdc.c
+++ b/drivers/video/stm32/stm32_ltdc.c
@@ -4,22 +4,20 @@
  * Author(s): Philippe Cornu <philippe.cornu@st.com> for STMicroelectronics.
  *	      Yannick Fertre <yannick.fertre@st.com> for STMicroelectronics.
  */
-
 #include <common.h>
 #include <clk.h>
+#include <display.h>
 #include <dm.h>
 #include <panel.h>
 #include <reset.h>
 #include <video.h>
+#include <video_bridge.h>
 #include <asm/io.h>
 #include <asm/arch/gpio.h>
 #include <dm/device-internal.h>
 
-DECLARE_GLOBAL_DATA_PTR;
-
 struct stm32_ltdc_priv {
 	void __iomem *regs;
-	struct display_timing timing;
 	enum video_log2_bpp l2bpp;
 	u32 bg_col_argb;
 	u32 crop_x, crop_y, crop_w, crop_h;
@@ -174,8 +172,8 @@ static u32 stm32_ltdc_get_pixel_format(enum video_log2_bpp l2bpp)
 	case VIDEO_BPP2:
 	case VIDEO_BPP4:
 	default:
-		debug("%s: warning %dbpp not supported yet, %dbpp instead\n",
-		      __func__, VNBITS(l2bpp), VNBITS(VIDEO_BPP16));
+		pr_warn("%s: warning %dbpp not supported yet, %dbpp instead\n",
+			__func__, VNBITS(l2bpp), VNBITS(VIDEO_BPP16));
 		pf = PF_RGB565;
 		break;
 	}
@@ -209,23 +207,23 @@ static void stm32_ltdc_enable(struct stm32_ltdc_priv *priv)
 	setbits_le32(priv->regs + LTDC_GCR, GCR_LTDCEN);
 }
 
-static void stm32_ltdc_set_mode(struct stm32_ltdc_priv *priv)
+static void stm32_ltdc_set_mode(struct stm32_ltdc_priv *priv,
+				struct display_timing *timings)
 {
 	void __iomem *regs = priv->regs;
-	struct display_timing *timing = &priv->timing;
 	u32 hsync, vsync, acc_hbp, acc_vbp, acc_act_w, acc_act_h;
 	u32 total_w, total_h;
 	u32 val;
 
 	/* Convert video timings to ltdc timings */
-	hsync = timing->hsync_len.typ - 1;
-	vsync = timing->vsync_len.typ - 1;
-	acc_hbp = hsync + timing->hback_porch.typ;
-	acc_vbp = vsync + timing->vback_porch.typ;
-	acc_act_w = acc_hbp + timing->hactive.typ;
-	acc_act_h = acc_vbp + timing->vactive.typ;
-	total_w = acc_act_w + timing->hfront_porch.typ;
-	total_h = acc_act_h + timing->vfront_porch.typ;
+	hsync = timings->hsync_len.typ - 1;
+	vsync = timings->vsync_len.typ - 1;
+	acc_hbp = hsync + timings->hback_porch.typ;
+	acc_vbp = vsync + timings->vback_porch.typ;
+	acc_act_w = acc_hbp + timings->hactive.typ;
+	acc_act_h = acc_vbp + timings->vactive.typ;
+	total_w = acc_act_w + timings->hfront_porch.typ;
+	total_h = acc_act_h + timings->vfront_porch.typ;
 
 	/* Synchronization sizes */
 	val = (hsync << 16) | vsync;
@@ -247,14 +245,14 @@ static void stm32_ltdc_set_mode(struct stm32_ltdc_priv *priv)
 
 	/* Signal polarities */
 	val = 0;
-	debug("%s: timing->flags 0x%08x\n", __func__, timing->flags);
-	if (timing->flags & DISPLAY_FLAGS_HSYNC_HIGH)
+	debug("%s: timing->flags 0x%08x\n", __func__, timings->flags);
+	if (timings->flags & DISPLAY_FLAGS_HSYNC_HIGH)
 		val |= GCR_HSPOL;
-	if (timing->flags & DISPLAY_FLAGS_VSYNC_HIGH)
+	if (timings->flags & DISPLAY_FLAGS_VSYNC_HIGH)
 		val |= GCR_VSPOL;
-	if (timing->flags & DISPLAY_FLAGS_DE_HIGH)
+	if (timings->flags & DISPLAY_FLAGS_DE_HIGH)
 		val |= GCR_DEPOL;
-	if (timing->flags & DISPLAY_FLAGS_PIXDATA_NEGEDGE)
+	if (timings->flags & DISPLAY_FLAGS_PIXDATA_NEGEDGE)
 		val |= GCR_PCPOL;
 	clrsetbits_le32(regs + LTDC_GCR,
 			GCR_HSPOL | GCR_VSPOL | GCR_DEPOL | GCR_PCPOL, val);
@@ -330,96 +328,129 @@ static int stm32_ltdc_probe(struct udevice *dev)
 	struct video_uc_platdata *uc_plat = dev_get_uclass_platdata(dev);
 	struct video_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct stm32_ltdc_priv *priv = dev_get_priv(dev);
-	struct udevice *panel;
+#ifdef CONFIG_VIDEO_BRIDGE
+	struct udevice *bridge = NULL;
+#endif
+	struct udevice *panel = NULL;
+	struct display_timing timings;
 	struct clk pclk;
 	struct reset_ctl rst;
-	int rate, ret;
+	int ret;
 
 	priv->regs = (void *)dev_read_addr(dev);
 	if ((fdt_addr_t)priv->regs == FDT_ADDR_T_NONE) {
-		debug("%s: ltdc dt register address error\n", __func__);
+		dev_err(dev, "ltdc dt register address error\n");
 		return -EINVAL;
 	}
 
 	ret = clk_get_by_index(dev, 0, &pclk);
 	if (ret) {
-		debug("%s: peripheral clock get error %d\n", __func__, ret);
+		dev_err(dev, "peripheral clock get error %d\n", ret);
 		return ret;
 	}
 
 	ret = clk_enable(&pclk);
 	if (ret) {
-		debug("%s: peripheral clock enable error %d\n",
-		      __func__, ret);
+		dev_err(dev, "peripheral clock enable error %d\n", ret);
 		return ret;
 	}
 
-	ret = reset_get_by_index(dev, 0, &rst);
+	ret = uclass_first_device_err(UCLASS_PANEL, &panel);
 	if (ret) {
-		debug("%s: missing ltdc hardware reset\n", __func__);
-		return -ENODEV;
+		if (ret != -ENODEV)
+			dev_err(dev, "panel device error %d\n", ret);
+		return ret;
 	}
 
-	/* Reset */
-	reset_deassert(&rst);
-
-	ret = uclass_first_device(UCLASS_PANEL, &panel);
+	ret = panel_get_display_timing(panel, &timings);
 	if (ret) {
-		debug("%s: panel device error %d\n", __func__, ret);
-		return ret;
+		ret = fdtdec_decode_display_timing(gd->fdt_blob,
+						   dev_of_offset(panel),
+						   0, &timings);
+		if (ret) {
+			dev_err(dev, "decode display timing error %d\n", ret);
+			return ret;
+		}
 	}
 
-	ret = panel_enable_backlight(panel);
+	ret = clk_set_rate(&pclk, timings.pixelclock.typ);
+	if (ret)
+		dev_warn(dev, "fail to set pixel clock %d hz\n",
+			 timings.pixelclock.typ);
+
+	debug("%s: Set pixel clock req %d hz get %ld hz\n", __func__,
+	      timings.pixelclock.typ, clk_get_rate(&pclk));
+
+	ret = reset_get_by_index(dev, 0, &rst);
 	if (ret) {
-		debug("%s: panel %s enable backlight error %d\n",
-		      __func__, panel->name, ret);
+		dev_err(dev, "missing ltdc hardware reset\n");
 		return ret;
 	}
 
-	ret = fdtdec_decode_display_timing(gd->fdt_blob,
-					   dev_of_offset(dev), 0,
-					   &priv->timing);
-	if (ret) {
-		debug("%s: decode display timing error %d\n",
-		      __func__, ret);
-		return -EINVAL;
-	}
+	/* Reset */
+	reset_deassert(&rst);
 
-	rate = clk_set_rate(&pclk, priv->timing.pixelclock.typ);
-	if (rate < 0) {
-		debug("%s: fail to set pixel clock %d hz %d hz\n",
-		      __func__, priv->timing.pixelclock.typ, rate);
-		return rate;
+#ifdef CONFIG_VIDEO_BRIDGE
+	ret = uclass_get_device(UCLASS_VIDEO_BRIDGE, 0, &bridge);
+	if (ret)
+		debug("No video bridge, or no backlight on bridge\n");
+
+	if (bridge) {
+		ret = video_bridge_attach(bridge);
+		if (ret) {
+			dev_err(dev, "fail to attach bridge\n");
+			return ret;
+		}
 	}
-
-	debug("%s: Set pixel clock req %d hz get %d hz\n", __func__,
-	      priv->timing.pixelclock.typ, rate);
-
+#endif
 	/* TODO Below parameters are hard-coded for the moment... */
 	priv->l2bpp = VIDEO_BPP16;
 	priv->bg_col_argb = 0xFFFFFFFF; /* white no transparency */
 	priv->crop_x = 0;
 	priv->crop_y = 0;
-	priv->crop_w = priv->timing.hactive.typ;
-	priv->crop_h = priv->timing.vactive.typ;
+	priv->crop_w = timings.hactive.typ;
+	priv->crop_h = timings.vactive.typ;
 	priv->alpha = 0xFF;
 
 	debug("%s: %dx%d %dbpp frame buffer at 0x%lx\n", __func__,
-	      priv->timing.hactive.typ, priv->timing.vactive.typ,
+	      timings.hactive.typ, timings.vactive.typ,
 	      VNBITS(priv->l2bpp), uc_plat->base);
 	debug("%s: crop %d,%d %dx%d bg 0x%08x alpha %d\n", __func__,
 	      priv->crop_x, priv->crop_y, priv->crop_w, priv->crop_h,
 	      priv->bg_col_argb, priv->alpha);
 
 	/* Configure & start LTDC */
-	stm32_ltdc_set_mode(priv);
+	stm32_ltdc_set_mode(priv, &timings);
 	stm32_ltdc_set_layer1(priv, uc_plat->base);
 	stm32_ltdc_enable(priv);
 
-	uc_priv->xsize = priv->timing.hactive.typ;
-	uc_priv->ysize = priv->timing.vactive.typ;
+	uc_priv->xsize = timings.hactive.typ;
+	uc_priv->ysize = timings.vactive.typ;
 	uc_priv->bpix = priv->l2bpp;
 
+#ifdef CONFIG_VIDEO_BRIDGE
+	if (bridge) {
+		ret = video_bridge_set_backlight(bridge, 80);
+		if (ret) {
+			dev_err(dev, "fail to set backlight\n");
+			return ret;
+		}
+	} else {
+		ret = panel_enable_backlight(panel);
+		if (ret) {
+			dev_err(dev, "panel %s enable backlight error %d\n",
+				panel->name, ret);
+			return ret;
+		}
+	}
+#else
+	ret = panel_enable_backlight(panel);
+	if (ret) {
+		dev_err(dev, "panel %s enable backlight error %d\n",
+			panel->name, ret);
+		return ret;
+	}
+#endif
 	video_set_flush_dcache(dev, true);
 
 	return 0;
diff --git a/drivers/video/video-uclass.c b/drivers/video/video-uclass.c
index 44dfa71..5fe49a5 100644
--- a/drivers/video/video-uclass.c
+++ b/drivers/video/video-uclass.c
@@ -300,3 +300,17 @@ UCLASS_DRIVER(video) = {
 	.per_device_auto_alloc_size	= sizeof(struct video_priv),
 	.per_device_platdata_auto_alloc_size = sizeof(struct video_uc_platdata),
 };
+
+static int do_video_clear(cmd_tbl_t *cmdtp, int flag, int argc,
+			  char *const argv[])
+{
+	struct udevice *dev;
+
+	if (uclass_first_device_err(UCLASS_VIDEO, &dev))
+		return CMD_RET_FAILURE;
+	video_clear(dev);
+
+	return 0;
+}
+
+U_BOOT_CMD(cls,	1, 1, do_video_clear, "clear screen", "");
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 02f4e1e..705fc7d 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -103,6 +103,21 @@ config WDT_CDNS
 	   Select this to enable Cadence watchdog timer, which can be found on some
 	   Xilinx Microzed Platform.
 
+config STM32MP_WATCHDOG
+	bool "Enable IWDG watchdog driver for STM32 MP's family"
+	depends on ARCH_STM32MP
+	select HW_WATCHDOG
+	help
+		Enable the STM32 watchdog (IWDG) driver. Enable support to
+		configure STM32's on-SoC watchdog.
+
+config STM32MP_WATCHDOG_TIMEOUT_SECS
+	int "IWDG watchdog timeout"
+	depends on STM32MP_WATCHDOG
+	default 32
+	help
+		Configure the timeout for IWDG (default: 32s).
+
 config XILINX_TB_WATCHDOG
 	bool "Xilinx Axi watchdog timer support"
 	depends on WDT
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 08406ca..25491cf 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -23,3 +23,4 @@ obj-$(CONFIG_BCM2835_WDT)       += bcm2835_wdt.o
 obj-$(CONFIG_WDT_ORION) += orion_wdt.o
 obj-$(CONFIG_WDT_CDNS) += cdns_wdt.o
 obj-$(CONFIG_MPC8xx_WATCHDOG) += mpc8xx_wdt.o
+obj-$(CONFIG_STM32MP_WATCHDOG) += stm32mp_wdt.o
\ No newline at end of file
diff --git a/drivers/watchdog/stm32mp_wdt.c b/drivers/watchdog/stm32mp_wdt.c
new file mode 100644
index 0000000..696be1b
--- /dev/null
+++ b/drivers/watchdog/stm32mp_wdt.c
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <regmap.h>
+#include <syscon.h>
+#include <watchdog.h>
+#include <asm/io.h>
+#include <asm/arch/stm32.h>
+
+/* IWDG registers */
+#define IWDG_KR		0x00	/* Key register */
+#define IWDG_PR		0x04	/* Prescaler Register */
+#define IWDG_RLR	0x08	/* ReLoad Register */
+#define IWDG_SR		0x0C	/* Status Register */
+
+/* IWDG_KR register bit mask */
+#define KR_KEY_RELOAD	0xAAAA	/* Reload counter enable */
+#define KR_KEY_ENABLE	0xCCCC	/* Peripheral enable */
+#define KR_KEY_EWA	0x5555	/* Write access enable */
+
+/* IWDG_PR register bit values */
+#define PR_256		0x06	/* Prescaler set to 256 */
+
+/* IWDG_RLR register values */
+#define RLR_MAX		0xFFF	/* Max value supported by reload register */
+
+static fdt_addr_t stm32mp_wdt_base
+	__attribute__((section(".data"))) = FDT_ADDR_T_NONE;
+
+void hw_watchdog_reset(void)
+{
+	if (stm32mp_wdt_base != FDT_ADDR_T_NONE)
+		writel(KR_KEY_RELOAD, stm32mp_wdt_base + IWDG_KR);
+}
+
+void hw_watchdog_init(void)
+{
+	struct regmap *map;
+
+	map = syscon_get_regmap_by_driver_data(STM32MP_SYSCON_IWDG);
+	if (!IS_ERR(map))
+		stm32mp_wdt_base = map->ranges[0].start;
+	else
+		printf("%s: iwdg init error", __func__);
+}
+
+static int stm32mp_wdt_probe(struct udevice *dev)
+{
+	struct regmap *map = syscon_get_regmap(dev);
+	struct clk clk;
+	int ret, reload;
+	u32 time_start;
+	ulong regmap_base = map->ranges[0].start;
+
+	debug("IWDG init\n");
+
+	/* Enable clock */
+	ret = clk_get_by_name(dev, "pclk", &clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	/* Get LSI clock */
+	ret = clk_get_by_name(dev, "lsi", &clk);
+	if (ret)
+		return ret;
+
+	/* Prescaler fixed to 256 */
+	reload = CONFIG_STM32MP_WATCHDOG_TIMEOUT_SECS *
+		 clk_get_rate(&clk) / 256;
+	if (reload > RLR_MAX + 1)
+		/* Force to max watchdog counter reload value */
+		reload = RLR_MAX + 1;
+	else if (!reload)
+		/* Force to min watchdog counter reload value */
+		reload = clk_get_rate(&clk) / 256;
+
+	/* Enable watchdog */
+	writel(KR_KEY_ENABLE, regmap_base + IWDG_KR);
+
+	/* Set prescaler & reload registers */
+	writel(KR_KEY_EWA, regmap_base + IWDG_KR);
+	writel(PR_256, regmap_base + IWDG_PR);
+	writel(reload - 1, regmap_base + IWDG_RLR);
+
+	/* Wait for the registers to be updated */
+	time_start = get_timer(0);
+	while (readl(regmap_base + IWDG_SR)) {
+		if (get_timer(time_start) > CONFIG_SYS_HZ) {
+			pr_err("Updating IWDG registers timeout");
+			return -ETIMEDOUT;
+		}
+	}
+
+	debug("IWDG init done\n");
+
+	return 0;
+}
+
+static const struct udevice_id stm32mp_wdt_match[] = {
+	{ .compatible = "st,stm32mp1-iwdg",
+	  .data = STM32MP_SYSCON_IWDG },
+	{ /* sentinel */ }
+};
+
+U_BOOT_DRIVER(stm32mp_wdt) = {
+	.name = "stm32mp-wdt",
+	.id = UCLASS_SYSCON,
+	.of_match = stm32mp_wdt_match,
+	.probe = stm32mp_wdt_probe,
+};
diff --git a/include/adc.h b/include/adc.h
index d04c9c4..5841dfb 100644
--- a/include/adc.h
+++ b/include/adc.h
@@ -219,6 +219,17 @@ int adc_channels_data(struct udevice *dev, unsigned int channel_mask,
 int adc_data_mask(struct udevice *dev, unsigned int *data_mask);
 
 /**
+ * adc_channel_mask() - get channel mask for given ADC device
+ *
+ * This can be used if adc uclass platform data is filled.
+ *
+ * @dev:       ADC device to check
+ * @channel_mask: pointer to the returned channel bitmask
+ * @return: 0 if OK, -ve on error
+ */
+int adc_channel_mask(struct udevice *dev, unsigned int *channel_mask);
+
+/**
  * adc_channel_single_shot() - get output data of conversion for the ADC
  * device's channel. This function searches for the device with the given name,
  * starts the given channel conversion and returns the output data.
@@ -284,4 +295,14 @@ int adc_vss_value(struct udevice *dev, int *uV);
  */
 int adc_stop(struct udevice *dev);
 
+/**
+ * adc_raw_to_uV() - converts raw value to microvolts for given ADC device.
+ *
+ * @dev:     ADC device used from conversion
+ * @raw:     raw value to convert
+ * @uV:	     converted value in microvolts
+ * @return:  0 on success or -ve on error
+ */
+int adc_raw_to_uV(struct udevice *dev, unsigned int raw, int *uV);
+
 #endif
diff --git a/include/configs/stm32mp1.h b/include/configs/stm32mp1.h
index 701298c..b4beaa7 100644
--- a/include/configs/stm32mp1.h
+++ b/include/configs/stm32mp1.h
@@ -10,17 +10,17 @@
 #include <linux/sizes.h>
 #include <asm/arch/stm32.h>
 
-#define CONFIG_PREBOOT
-
 /*
  * Number of clock ticks in 1 sec
  */
 #define CONFIG_SYS_HZ				1000
 
+#ifndef CONFIG_STM32MP1_TRUSTED
 /* PSCI support */
 #define CONFIG_ARMV7_PSCI_1_0
 #define CONFIG_ARMV7_SECURE_BASE		STM32_SYSRAM_BASE
 #define CONFIG_ARMV7_SECURE_MAX_SIZE		STM32_SYSRAM_SIZE
+#endif
 
 /*
  * malloc() pool size
@@ -33,6 +33,8 @@
 #define CONFIG_SYS_SDRAM_BASE			STM32_DDR_BASE
 #define CONFIG_SYS_INIT_SP_ADDR			CONFIG_SYS_TEXT_BASE
 
+#define CONFIG_DISABLE_CONSOLE
+
 /*
  * Console I/O buffer size
  */
@@ -53,6 +55,9 @@
 #define CONFIG_SETUP_MEMORY_TAGS
 #define CONFIG_INITRD_TAG
 
+/* Extend size of kernel image for uncompression */
+#define CONFIG_SYS_BOOTM_LEN			SZ_32M
+
 /* SPL support */
 #ifdef CONFIG_SPL
 /* BOOTROM load address */
@@ -69,36 +74,118 @@
 					 STM32_SYSRAM_SIZE)
 #endif /* #ifdef CONFIG_SPL */
 
+#define CONFIG_SYS_MEMTEST_START	STM32_DDR_BASE
+#define CONFIG_SYS_MEMTEST_END		(CONFIG_SYS_MEMTEST_START + SZ_64M)
+#define CONFIG_SYS_MEMTEST_SCRATCH	(CONFIG_SYS_MEMTEST_END + 4)
+
 /*MMC SD*/
 #define CONFIG_SYS_MMC_MAX_DEVICE	3
 #define CONFIG_SUPPORT_EMMC_BOOT
 
-#if !defined(CONFIG_SPL) || !defined(CONFIG_SPL_BUILD)
+/*****************************************************************************/
+#ifdef CONFIG_DISTRO_DEFAULTS
+/*****************************************************************************/
+
+/* NAND support */
+#define CONFIG_SYS_NAND_ONFI_DETECTION
+#define CONFIG_SYS_MAX_NAND_DEVICE	1
+
+/* SPI nand */
+#define CONFIG_SYS_MAX_NAND_DEVICE	1
+
+/* SPI FLASH support */
+#if defined(CONFIG_SPL_BUILD)
+#define CONFIG_SYS_SPI_U_BOOT_OFFS	0x80000
+#endif
 
+/* FILE SYSTEM */
+
+#if defined(CONFIG_STM32_QSPI) || defined(CONFIG_NAND_STM32_FMC2)
+/* Dynamic MTD partition support */
+#define CONFIG_SYS_MTDPARTS_RUNTIME
+#endif
+
+/* Ethernet need */
+#ifdef CONFIG_DWC_ETH_QOS
+#define CONFIG_SYS_NONCACHED_MEMORY	(1 * SZ_1M)	/* 1M */
+#define CONFIG_SERVERIP			192.168.1.1
+#define CONFIG_BOOTP_SERVERIP
+#define CONFIG_SYS_AUTOLOAD		"no"
+#endif
+
+#ifdef CONFIG_DM_VIDEO
+#define CONFIG_VIDEO_BMP_RLE8
+#define CONFIG_BMP_16BPP
+#define CONFIG_BMP_24BPP
+#define CONFIG_BMP_32BPP
+#endif
+
+#if !defined(CONFIG_SPL_BUILD)
+
+/* default order is eMMC (SDMMC 1)/ NAND / SDCARD (SDMMC 0) / SDMMC2 */
 #define BOOT_TARGET_DEVICES(func) \
 	func(MMC, mmc, 1) \
+	func(UBIFS, ubifs, 0) \
 	func(MMC, mmc, 0) \
-	func(MMC, mmc, 2)
+	func(MMC, mmc, 2) \
+	func(PXE, pxe, na)
 
 #include <config_distro_bootcmd.h>
 
-#define STM32MP_PREBOOT	\
+#define CONFIG_PREBOOT \
 	"echo \"Boot over ${boot_device}${boot_instance}!\"; " \
-	"if test \"${boot_device}\" = \"mmc\"; then " \
-		"env set boot_targets \"mmc${boot_instance}\"; "\
-	"fi;"
+	"if test ${boot_device} = serial; then " \
+		"stm32prog serial ${boot_instance}; " \
+	"else if test ${boot_device} = usb; then " \
+		"stm32prog usb ${boot_instance}; " \
+	"else " \
+		"if test ${boot_device} = mmc; then " \
+			"env set boot_targets \"mmc${boot_instance}\"; "\
+		"else if test ${boot_device} = nand; then " \
+			"env set boot_targets \"ubifs0\"; "\
+	"fi; fi; fi; fi;"
+
+#ifdef CONFIG_STM32MP1_OPTEE
+#define CONFIG_SYS_MEM_TOP_HIDE			SZ_32M
+/* with OPTEE: define specific MTD partitions = teeh, teed, teex */
+#define STM32MP_MTDPARTS \
+	"mtdparts_nor0=256k(fsbl1),256k(fsbl2),2m(ssbl),256k(logo),256k(teeh),256k(teed),256k(teex),-(nor_user)\0" \
+	"mtdparts_nand0=2m(fsbl),2m(ssbl1),2m(ssbl2),512k(teeh),512k(teed),512k(teex),-(UBI);\0"
+
+#else /* CONFIG_STM32MP1_OPTEE */
+
+#define STM32MP_MTDPARTS \
+	"mtdparts_nor0=256k(fsbl1),256k(fsbl2),2m(ssbl),256k(logo),-(nor_user)\0" \
+	"mtdparts_nand0=2m(fsbl),2m(ssbl1),2m(ssbl2),-(UBI)\0"
+
+#endif /* CONFIG_STM32MP1_OPTEE */
 
+/*
+ * memory layout for 32M uncompressed/compressed kernel,
+ * 1M fdt, 1M script, 1M pxe and 1M for splashimage
+ * and the ramdisk at the end.
+ */
 #define CONFIG_EXTRA_ENV_SETTINGS \
-	"scriptaddr=0xC0000000\0" \
-	"pxefile_addr_r=0xC0000000\0" \
-	"kernel_addr_r=0xC1000000\0" \
-	"fdt_addr_r=0xC4000000\0" \
-	"ramdisk_addr_r=0xC4100000\0" \
+	"stdin=serial\0" \
+	"stdout=serial\0" \
+	"stderr=serial\0" \
+	"bootdelay=1\0" \
+	"kernel_addr_r=0xc2000000\0" \
+	"fdt_addr_r=0xc4000000\0" \
+	"scriptaddr=0xc4100000\0" \
+	"pxefile_addr_r=0xc4200000\0" \
+	"splashimage=0xc4300000\0"  \
+	"ramdisk_addr_r=0xc4400000\0" \
 	"fdt_high=0xffffffff\0" \
 	"initrd_high=0xffffffff\0" \
-	"preboot=" STM32MP_PREBOOT "\0" \
-	BOOTENV
+	"bootlimit=0\0" \
+	"altbootcmd=run bootcmd\0" \
+	"usb_pgood_delay=2000\0" \
+	STM32MP_MTDPARTS \
+	BOOTENV \
+	"boot_net_usb_start=true\0"
 
 #endif /* ifndef CONFIG_SPL_BUILD */
+#endif /* ifdef CONFIG_DISTRO_DEFAULTS*/
 
 #endif /* __CONFIG_H */
diff --git a/include/dfu.h b/include/dfu.h
index fbe978a..36304c7 100644
--- a/include/dfu.h
+++ b/include/dfu.h
@@ -22,6 +22,7 @@ enum dfu_device_type {
 	DFU_DEV_NAND,
 	DFU_DEV_RAM,
 	DFU_DEV_SF,
+	DFU_DEV_VIRT,
 };
 
 enum dfu_layout {
@@ -77,6 +78,12 @@ struct sf_internal_data {
 	/* RAW programming */
 	u64 start;
 	u64 size;
+	/* for sf/ubi use */
+	unsigned int ubi;
+};
+
+struct virt_internal_data {
+	int dev_num;
 };
 
 #define DFU_NAME_SIZE			32
@@ -107,6 +114,7 @@ struct dfu_entity {
 		struct nand_internal_data nand;
 		struct ram_internal_data ram;
 		struct sf_internal_data sf;
+		struct virt_internal_data virt;
 	} data;
 
 	int (*get_medium_size)(struct dfu_entity *dfu, u64 *size);
@@ -142,6 +150,8 @@ struct dfu_entity {
 #ifdef CONFIG_SET_DFU_ALT_INFO
 void set_dfu_alt_info(char *interface, char *devstr);
 #endif
+int dfu_alt_init(int num, struct dfu_entity **dfu);
+int dfu_alt_add(struct dfu_entity *dfu, char *interface, char *devstr, char *s);
 int dfu_config_entities(char *s, char *interface, char *devstr);
 void dfu_free_entities(void);
 void dfu_show_entities(void);
@@ -161,6 +171,9 @@ bool dfu_usb_get_reset(void);
 int dfu_read(struct dfu_entity *de, void *buf, int size, int blk_seq_num);
 int dfu_write(struct dfu_entity *de, void *buf, int size, int blk_seq_num);
 int dfu_flush(struct dfu_entity *de, void *buf, int size, int blk_seq_num);
+void dfu_flush_callback(struct dfu_entity *dfu);
+void dfu_transaction_cleanup(struct dfu_entity *dfu);
+int dfu_transaction_initiate(struct dfu_entity *dfu, bool read);
 
 /*
  * dfu_defer_flush - pointer to store dfu_entity for deferred flashing.
@@ -246,6 +259,22 @@ static inline int dfu_fill_entity_sf(struct dfu_entity *dfu, char *devstr,
 }
 #endif
 
+#ifdef CONFIG_DFU_VIRT
+int dfu_fill_entity_virt(struct dfu_entity *dfu, char *devstr, char *s);
+int dfu_write_medium_virt(struct dfu_entity *dfu, u64 offset,
+			  void *buf, long *len);
+int dfu_get_medium_size_virt(struct dfu_entity *dfu, u64 *size);
+int dfu_read_medium_virt(struct dfu_entity *dfu, u64 offset,
+			 void *buf, long *len);
+#else
+static inline int dfu_fill_entity_virt(struct dfu_entity *dfu, char *devstr,
+				       char *s)
+{
+	puts("VIRT support not available!\n");
+	return -1;
+}
+#endif
+
 /**
  * dfu_tftp_write - Write TFTP data to DFU medium
  *
diff --git a/include/dm/pinctrl.h b/include/dm/pinctrl.h
index 80de3f3..63a7d55 100644
--- a/include/dm/pinctrl.h
+++ b/include/dm/pinctrl.h
@@ -6,6 +6,9 @@
 #ifndef __PINCTRL_H
 #define __PINCTRL_H
 
+#define PINNAME_SIZE	10
+#define PINMUX_SIZE	40
+
 /**
  * struct pinconf_param - pin config parameters
  *
@@ -66,6 +69,7 @@ struct pinconf_param {
  *	pointing a config node. (necessary for pinctrl_full)
  * @set_state_simple: do needed pinctrl operations for a peripherl @periph.
  *	(necessary for pinctrl_simple)
+ * @get_pin_muxing: display the muxing of a given pin.
  */
 struct pinctrl_ops {
 	int (*get_pins_count)(struct udevice *dev);
@@ -129,6 +133,24 @@ struct pinctrl_ops {
 	* @return mux value (SoC-specific, e.g. 0 for input, 1 for output)
 	 */
 	int (*get_gpio_mux)(struct udevice *dev, int banknum, int index);
+
+	/**
+	 * get_pin_muxing() - show pin muxing
+	 *
+	 * This allows to display the muxing of a given pin. It's useful for
+	 * debug purpose to know if a pin is configured as GPIO or as an
+	 * alternate function and which one.
+	 * Typically it is used by a PINCTRL driver with knowledge of the SoC
+	 * pinctrl setup.
+	 *
+	 * @dev:	Pinctrl device to use
+	 * @selector:	Pin selector
+	 * @buf		Pin's muxing description
+	 * @size	Pin's muxing description length
+	 * return 0 if OK, -ve on error
+	 */
+	 int (*get_pin_muxing)(struct udevice *dev, unsigned int selector,
+			       char *buf, int size);
 };
 
 #define pinctrl_get_ops(dev)	((struct pinctrl_ops *)(dev)->driver->ops)
@@ -348,4 +370,41 @@ int pinctrl_decode_pin_config(const void *blob, int node);
 */
 int pinctrl_get_gpio_mux(struct udevice *dev, int banknum, int index);
 
+/**
+ * pinctrl_get_pin_muxing() - Returns the muxing description
+ *
+ * This allows to display the muxing description of the given pin for
+ * debug purpose
+ *
+ * @dev:	Pinctrl device to use
+ * @selector	Pin index within pin-controller
+ * @buf		Pin's muxing description
+ * @size	Pin's muxing description length
+ * @return 0 if OK, -ve on error
+ */
+int pinctrl_get_pin_muxing(struct udevice *dev, int selector, char *buf,
+			   int size);
+
+/**
+ * pinctrl_get_pins_count() - display pin-controller pins number
+ *
+ * This allows to know the number of pins owned by a given pin-controller
+ *
+ * @dev:	Pinctrl device to use
+ * @return pins number if OK, -ve on error
+ */
+int pinctrl_get_pins_count(struct udevice *dev);
+
+/**
+ * pinctrl_get_pin_name() - Returns the pin's name
+ *
+ * This allows to display the pin's name for debug purpose
+ *
+ * @dev:	Pinctrl device to use
+ * @selector	Pin index within pin-controller
+ * @buf		Pin's name
+ * @return 0 if OK, -ve on error
+ */
+int pinctrl_get_pin_name(struct udevice *dev, int selector, char *buf,
+			 int size);
 #endif /* __PINCTRL_H */
diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
index 269a2c6..6193017 100644
--- a/include/dm/uclass-id.h
+++ b/include/dm/uclass-id.h
@@ -40,6 +40,7 @@ enum uclass_id {
 	UCLASS_ETH,		/* Ethernet device */
 	UCLASS_FS_FIRMWARE_LOADER,		/* Generic loader */
 	UCLASS_GPIO,		/* Bank of general-purpose I/O pins */
+	UCLASS_HWSPINLOCK,	/* Hardware semaphores */
 	UCLASS_FIRMWARE,	/* Firmware */
 	UCLASS_I2C,		/* I2C bus */
 	UCLASS_I2C_EEPROM,	/* I2C EEPROM device */
diff --git a/include/dm/uclass.h b/include/dm/uclass.h
index eebf2d5..11e6df3 100644
--- a/include/dm/uclass.h
+++ b/include/dm/uclass.h
@@ -306,6 +306,18 @@ int uclass_first_device_err(enum uclass_id id, struct udevice **devp);
 int uclass_next_device(struct udevice **devp);
 
 /**
+ * uclass_next_device_err() - Get the next device in a uclass
+ *
+ * The device returned is probed if necessary, and ready for use
+ *
+ * @devp: On entry, pointer to device to lookup. On exit, returns pointer
+ * to the next device in the uclass if no error occurred, or -ENODEV if
+ * there is no next device.
+ * @return 0 if found, -ENODEV if not found, other -ve on error
+ */
+int uclass_next_device_err(struct udevice **devp);
+
+/**
  * uclass_first_device_check() - Get the first device in a uclass
  *
  * The device returned is probed if necessary, and ready for use
@@ -379,4 +391,20 @@ int uclass_resolve_seq(struct udevice *dev);
 #define uclass_foreach_dev_safe(pos, next, uc)	\
 	list_for_each_entry_safe(pos, next, &uc->dev_head, uclass_node)
 
+/**
+ * uclass_foreach_dev_probe() - Helper function to iteration through devices
+ * of given uclass
+ *
+ * This creates a for() loop which works through the available devices in
+ * a uclass in order from start to end. Devices are probed if necessary,
+ * and ready for use.
+ *
+ * @id: Uclass ID
+ * @dev: struct udevice * to hold the current device. Set to NULL when there
+ * are no more devices.
+ */
+#define uclass_foreach_dev_probe(id, dev)	\
+	for (int _ret = uclass_first_device_err(id, &dev); !_ret && dev; \
+	     _ret = uclass_next_device_err(&dev))
+
 #endif
diff --git a/include/dw_mipi_dsi.h b/include/dw_mipi_dsi.h
new file mode 100644
index 0000000..ebf4713
--- /dev/null
+++ b/include/dw_mipi_dsi.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2017-2018, STMicroelectronics - All Rights Reserved
+ *
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Philippe Cornu <philippe.cornu@st.com>
+ *
+ * This generic Synopsys DesignWare MIPI DSI host include is based on
+ * the Linux Kernel include from include/drm/bridge/dw_mipi_dsi.h.
+ */
+
+#ifndef __DW_MIPI_DSI__
+#define __DW_MIPI_DSI__
+
+#include <mipi_display.h>
+
+struct dw_mipi_dsi_phy_ops {
+	int (*init)(void *priv_data);
+	int (*get_lane_mbps)(void *priv_data, struct display_timing *timings,
+			     u32 lanes, u32 format, unsigned int *lane_mbps);
+};
+
+struct dw_mipi_dsi_plat_data {
+	unsigned int max_data_lanes;
+	const struct dw_mipi_dsi_phy_ops *phy_ops;
+	struct udevice *panel;
+};
+
+int dw_mipi_dsi_init_bridge(struct mipi_dsi_device *device);
+void dw_mipi_dsi_bridge_enable(struct mipi_dsi_device *device);
+
+#endif /* __DW_MIPI_DSI__ */
diff --git a/include/g_dnl.h b/include/g_dnl.h
index 6d461c7..836ee60 100644
--- a/include/g_dnl.h
+++ b/include/g_dnl.h
@@ -38,6 +38,7 @@ int g_dnl_board_usb_cable_connected(void);
 int g_dnl_register(const char *s);
 void g_dnl_unregister(void);
 void g_dnl_set_serialnumber(char *);
+void g_dnl_set_product(const char *s);
 
 bool g_dnl_detach(void);
 void g_dnl_trigger_detach(void);
diff --git a/include/hwspinlock.h b/include/hwspinlock.h
new file mode 100644
index 0000000..99389c1
--- /dev/null
+++ b/include/hwspinlock.h
@@ -0,0 +1,140 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef _HWSPINLOCK_H_
+#define _HWSPINLOCK_H_
+
+/**
+ * Implement a hwspinlock uclass.
+ * Hardware spinlocks are used to perform hardware protection of
+ * critical sections and synchronisation between multiprocessors.
+ */
+
+struct udevice;
+
+/**
+ * struct hwspinlock - A handle to (allowing control of) a single hardware
+ * spinlock.
+ *
+ * @dev: The device which implements the hardware spinlock.
+ * @id: The hardware spinlock ID within the provider.
+ */
+struct hwspinlock {
+	struct udevice *dev;
+	unsigned long id;
+};
+
+#if CONFIG_IS_ENABLED(DM_HWSPINLOCK)
+
+/**
+ * hwspinlock_get_by_index - Get a hardware spinlock by integer index
+ *
+ * This looks up and request a hardware spinlock. The index is relative to the
+ * client device; each device is assumed to have n hardware spinlock associated
+ * with it somehow, and this function finds and requests one of them.
+ *
+ * @dev:	The client device.
+ * @index:	The index of the hardware spinlock to request, within the
+ *		client's list of hardware spinlock.
+ * @hws:	A pointer to a hardware spinlock struct to initialize.
+ * @return 0 if OK, or a negative error code.
+ */
+int hwspinlock_get_by_index(struct udevice *dev,
+			    int index, struct hwspinlock *hws);
+
+/**
+ * Lock the hardware spinlock
+ *
+ * @hws:	A hardware spinlock struct that previously requested by
+ *		hwspinlock_get_by_index
+ * @timeout:	Timeout value in msecs
+ * @return: 0 if OK, -ETIMEDOUT if timeout, -ve on other errors
+ */
+int hwspinlock_lock_timeout(struct hwspinlock *hws, unsigned int timeout);
+
+/**
+ * Unlock the hardware spinlock
+ *
+ * @hws:	A hardware spinlock struct that previously requested by
+ *		hwspinlock_get_by_index
+ * @return: 0 if OK, -ve on error
+ */
+int hwspinlock_unlock(struct hwspinlock *hws);
+
+#else
+
+static inline int hwspinlock_get_by_index(struct udevice *dev,
+					  int index,
+					  struct hwspinlock *hws)
+{
+	return -ENOSYS;
+}
+
+static inline int hwspinlock_lock_timeout(struct hwspinlock *hws,
+					  int timeout)
+{
+	return -ENOSYS;
+}
+
+static inline int hwspinlock_unlock(struct hwspinlock *hws)
+{
+	return -ENOSYS;
+}
+
+#endif /* CONFIG_DM_HWSPINLOCK */
+
+struct ofnode_phandle_args;
+
+/**
+ * struct hwspinlock_ops - Driver model hwspinlock operations
+ *
+ * The uclass interface is implemented by all hwspinlock devices which use
+ * driver model.
+ */
+struct hwspinlock_ops {
+	/**
+	 * of_xlate - Translate a client's device-tree (OF) hardware specifier.
+	 *
+	 * The hardware core calls this function as the first step in
+	 * implementing a client's hwspinlock_get_by_*() call.
+	 *
+	 * @hws:	The hardware spinlock struct to hold the translation
+	 *			result.
+	 * @args:	The hardware spinlock specifier values from device tree.
+	 * @return 0 if OK, or a negative error code.
+	 */
+	int (*of_xlate)(struct hwspinlock *hws,
+			struct ofnode_phandle_args *args);
+
+	/**
+	 * Lock the hardware spinlock
+	 *
+	 * @dev:	hwspinlock Device
+	 * @index:	index of the lock to be used
+	 * @return 0 if OK, -ve on error
+	 */
+	int (*lock)(struct udevice *dev, int index);
+
+	/**
+	 * Unlock the hardware spinlock
+	 *
+	 * @dev:	hwspinlock Device
+	 * @index:	index of the lock to be unlocked
+	 * @return 0 if OK, -ve on error
+	 */
+	int (*unlock)(struct udevice *dev, int index);
+
+	/**
+	 * Relax - optional
+	 *       Platform-specific relax method, called by hwspinlock core
+	 *       while spinning on a lock, between two successive call to
+	 *       lock
+	 *
+	 * @dev:	hwspinlock Device
+	 */
+	void (*relax)(struct udevice *dev);
+};
+
+#endif /* _HWSPINLOCK_H_ */
diff --git a/include/image.h b/include/image.h
index 031c355..610da44 100644
--- a/include/image.h
+++ b/include/image.h
@@ -278,6 +278,7 @@ enum {
 	IH_TYPE_PMMC,            /* TI Power Management Micro-Controller Firmware */
 	IH_TYPE_STM32IMAGE,		/* STMicroelectronics STM32 Image */
 	IH_TYPE_SOCFPGAIMAGE_V1,	/* Altera SOCFPGA A10 Preloader	*/
+	IH_TYPE_STM32COPRO,		/* STMicroelectronics STM32 Coprocessor Image */
 
 	IH_TYPE_COUNT,			/* Number of image types */
 };
diff --git a/include/mipi_display.h b/include/mipi_display.h
index ddcc8ca..5c3dbbe 100644
--- a/include/mipi_display.h
+++ b/include/mipi_display.h
@@ -4,12 +4,16 @@
  *
  * Copyright (C) 2010 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
  * Copyright (C) 2006 Nokia Corporation
- * Author: Imre Deak <imre.deak@nokia.com>
+ * Copyright (C) 2018 STMicroelectronics - All Rights Reserved
+ * Author(s): Imre Deak <imre.deak@nokia.com>
+ *            Yannick Fertre <yannick.fertre@st.com>
+ *            Philippe Cornu <philippe.cornu@st.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+
 #ifndef MIPI_DISPLAY_H
 #define MIPI_DISPLAY_H
 
@@ -115,6 +119,14 @@ enum {
 	MIPI_DCS_READ_MEMORY_CONTINUE	= 0x3E,
 	MIPI_DCS_SET_TEAR_SCANLINE	= 0x44,
 	MIPI_DCS_GET_SCANLINE		= 0x45,
+	MIPI_DCS_SET_DISPLAY_BRIGHTNESS = 0x51,		/* MIPI DCS 1.3 */
+	MIPI_DCS_GET_DISPLAY_BRIGHTNESS = 0x52,		/* MIPI DCS 1.3 */
+	MIPI_DCS_WRITE_CONTROL_DISPLAY  = 0x53,		/* MIPI DCS 1.3 */
+	MIPI_DCS_GET_CONTROL_DISPLAY	= 0x54,		/* MIPI DCS 1.3 */
+	MIPI_DCS_WRITE_POWER_SAVE	= 0x55,		/* MIPI DCS 1.3 */
+	MIPI_DCS_GET_POWER_SAVE		= 0x56,		/* MIPI DCS 1.3 */
+	MIPI_DCS_SET_CABC_MIN_BRIGHTNESS = 0x5E,	/* MIPI DCS 1.3 */
+	MIPI_DCS_GET_CABC_MIN_BRIGHTNESS = 0x5F,	/* MIPI DCS 1.3 */
 	MIPI_DCS_READ_DDB_START		= 0xA1,
 	MIPI_DCS_READ_DDB_CONTINUE	= 0xA8,
 };
@@ -127,4 +139,247 @@ enum {
 #define MIPI_DCS_PIXEL_FMT_8BIT		2
 #define MIPI_DCS_PIXEL_FMT_3BIT		1
 
+struct mipi_dsi_host;
+struct mipi_dsi_device;
+
+/* request ACK from peripheral */
+#define MIPI_DSI_MSG_REQ_ACK	BIT(0)
+/* use Low Power Mode to transmit message */
+#define MIPI_DSI_MSG_USE_LPM	BIT(1)
+
+/**
+ * struct mipi_dsi_msg - read/write DSI buffer
+ * @channel: virtual channel id
+ * @type: payload data type
+ * @flags: flags controlling this message transmission
+ * @tx_len: length of @tx_buf
+ * @tx_buf: data to be written
+ * @rx_len: length of @rx_buf
+ * @rx_buf: data to be read, or NULL
+ */
+struct mipi_dsi_msg {
+	u8 channel;
+	u8 type;
+	u16 flags;
+
+	size_t tx_len;
+	const void *tx_buf;
+
+	size_t rx_len;
+	void *rx_buf;
+};
+
+bool mipi_dsi_packet_format_is_short(u8 type);
+bool mipi_dsi_packet_format_is_long(u8 type);
+
+/**
+ * struct mipi_dsi_packet - represents a MIPI DSI packet in protocol format
+ * @size: size (in bytes) of the packet
+ * @header: the four bytes that make up the header (Data ID, Word Count or
+ *     Packet Data, and ECC)
+ * @payload_length: number of bytes in the payload
+ * @payload: a pointer to a buffer containing the payload, if any
+ */
+struct mipi_dsi_packet {
+	size_t size;
+	u8 header[4];
+	size_t payload_length;
+	const u8 *payload;
+};
+
+int mipi_dsi_create_packet(struct mipi_dsi_packet *packet,
+			   const struct mipi_dsi_msg *msg);
+
+/**
+ * struct mipi_dsi_host_ops - DSI bus operations
+ * @attach: attach DSI device to DSI host
+ * @detach: detach DSI device from DSI host
+ * @transfer: transmit a DSI packet
+ *
+ * DSI packets transmitted by .transfer() are passed in as mipi_dsi_msg
+ * structures. This structure contains information about the type of packet
+ * being transmitted as well as the transmit and receive buffers. When an
+ * error is encountered during transmission, this function will return a
+ * negative error code. On success it shall return the number of bytes
+ * transmitted for write packets or the number of bytes received for read
+ * packets.
+ *
+ * Note that typically DSI packet transmission is atomic, so the .transfer()
+ * function will seldomly return anything other than the number of bytes
+ * contained in the transmit buffer on success.
+ */
+struct mipi_dsi_host_ops {
+	int (*attach)(struct mipi_dsi_host *host,
+		      struct mipi_dsi_device *dsi);
+	int (*detach)(struct mipi_dsi_host *host,
+		      struct mipi_dsi_device *dsi);
+	ssize_t (*transfer)(struct mipi_dsi_host *host,
+			    const struct mipi_dsi_msg *msg);
+};
+
+/**
+ * struct mipi_dsi_host - DSI host device
+ * @dev: driver model device node for this DSI host
+ * @ops: DSI host operations
+ * @list: list management
+ */
+struct mipi_dsi_host {
+	struct device *dev;
+	const struct mipi_dsi_host_ops *ops;
+	struct list_head list;
+};
+
+/* DSI mode flags */
+
+/* video mode */
+#define MIPI_DSI_MODE_VIDEO		BIT(0)
+/* video burst mode */
+#define MIPI_DSI_MODE_VIDEO_BURST	BIT(1)
+/* video pulse mode */
+#define MIPI_DSI_MODE_VIDEO_SYNC_PULSE	BIT(2)
+/* enable auto vertical count mode */
+#define MIPI_DSI_MODE_VIDEO_AUTO_VERT	BIT(3)
+/* enable hsync-end packets in vsync-pulse and v-porch area */
+#define MIPI_DSI_MODE_VIDEO_HSE		BIT(4)
+/* disable hfront-porch area */
+#define MIPI_DSI_MODE_VIDEO_HFP		BIT(5)
+/* disable hback-porch area */
+#define MIPI_DSI_MODE_VIDEO_HBP		BIT(6)
+/* disable hsync-active area */
+#define MIPI_DSI_MODE_VIDEO_HSA		BIT(7)
+/* flush display FIFO on vsync pulse */
+#define MIPI_DSI_MODE_VSYNC_FLUSH	BIT(8)
+/* disable EoT packets in HS mode */
+#define MIPI_DSI_MODE_EOT_PACKET	BIT(9)
+/* device supports non-continuous clock behavior (DSI spec 5.6.1) */
+#define MIPI_DSI_CLOCK_NON_CONTINUOUS	BIT(10)
+/* transmit data in low power */
+#define MIPI_DSI_MODE_LPM		BIT(11)
+
+enum mipi_dsi_pixel_format {
+	MIPI_DSI_FMT_RGB888,
+	MIPI_DSI_FMT_RGB666,
+	MIPI_DSI_FMT_RGB666_PACKED,
+	MIPI_DSI_FMT_RGB565,
+};
+
+#define DSI_DEV_NAME_SIZE		20
+
+/**
+ * struct mipi_dsi_device_info - template for creating a mipi_dsi_device
+ * @type: DSI peripheral chip type
+ * @channel: DSI virtual channel assigned to peripheral
+ * @node: pointer to OF device node or NULL
+ *
+ * This is populated and passed to mipi_dsi_device_new to create a new
+ * DSI device
+ */
+struct mipi_dsi_device_info {
+	char type[DSI_DEV_NAME_SIZE];
+	u32 channel;
+	struct device_node *node;
+};
+
+/**
+ * struct mipi_dsi_device - DSI peripheral device
+ * @host: DSI host for this peripheral
+ * @dev: driver model device node for this peripheral
+ * @name: DSI peripheral chip type
+ * @channel: virtual channel assigned to the peripheral
+ * @format: pixel format for video mode
+ * @lanes: number of active data lanes
+ * @mode_flags: DSI operation mode related flags
+ */
+struct mipi_dsi_device {
+	struct mipi_dsi_host *host;
+	struct udevice *dev;
+
+	char name[DSI_DEV_NAME_SIZE];
+	unsigned int channel;
+	unsigned int lanes;
+	enum mipi_dsi_pixel_format format;
+	unsigned long mode_flags;
+};
+
+/**
+ * enum mipi_dsi_dcs_tear_mode - Tearing Effect Output Line mode
+ * @MIPI_DSI_DCS_TEAR_MODE_VBLANK: the TE output line consists of V-Blanking
+ *    information only
+ * @MIPI_DSI_DCS_TEAR_MODE_VHBLANK : the TE output line consists of both
+ *    V-Blanking and H-Blanking information
+ */
+enum mipi_dsi_dcs_tear_mode {
+	MIPI_DSI_DCS_TEAR_MODE_VBLANK,
+	MIPI_DSI_DCS_TEAR_MODE_VHBLANK,
+};
+
+/**
+ * struct mipi_dsi_panel_plat - DSI panel platform data
+ * @device: DSI peripheral device
+ */
+struct mipi_dsi_panel_plat {
+	struct mipi_dsi_device *device;
+};
+
+int mipi_dsi_attach(struct mipi_dsi_device *dsi);
+int mipi_dsi_detach(struct mipi_dsi_device *dsi);
+int mipi_dsi_pixel_format_to_bpp(enum mipi_dsi_pixel_format fmt);
+
+/* bit compatible with the xrandr RR_ definitions (bits 0-13)
+ *
+ * ABI warning: Existing userspace really expects
+ * the mode flags to match the xrandr definitions. Any
+ * changes that don't match the xrandr definitions will
+ * likely need a new client cap or some other mechanism
+ * to avoid breaking existing userspace. This includes
+ * allocating new flags in the previously unused bits!
+ */
+#define MIPI_DSI_FLAG_PHSYNC			BIT(0)
+#define MIPI_DSI_FLAG_NHSYNC			BIT(1)
+#define MIPI_DSI_FLAG_PVSYNC			BIT(2)
+#define MIPI_DSI_FLAG_NVSYNC			BIT(3)
+#define MIPI_DSI_FLAG_INTERLACE			BIT(4)
+#define MIPI_DSI_FLAG_DBLSCAN			BIT(5)
+#define MIPI_DSI_FLAG_CSYNC			BIT(6)
+#define MIPI_DSI_FLAG_PCSYNC			BIT(7)
+#define MIPI_DSI_FLAG_NCSYNC			BIT(8)
+#define MIPI_DSI_FLAG_HSKEW			BIT(9) /* hskew provided */
+#define MIPI_DSI_FLAG_BCAST			BIT(10)
+#define MIPI_DSI_FLAG_PIXMUX			BIT(11)
+#define MIPI_DSI_FLAG_DBLCLK			BIT(12)
+#define MIPI_DSI_FLAG_CLKDIV2			BIT(13)
+
+/* request ACK from peripheral */
+#define MIPI_DSI_MSG_REQ_ACK	BIT(0)
+/* use Low Power Mode to transmit message */
+#define MIPI_DSI_MSG_USE_LPM	BIT(1)
+
+ssize_t mipi_dsi_dcs_write_buffer(struct mipi_dsi_device *dsi,
+				  const void *data, size_t len);
+ssize_t mipi_dsi_dcs_write(struct mipi_dsi_device *dsi, u8 cmd,
+			   const void *data, size_t len);
+ssize_t mipi_dsi_dcs_read(struct mipi_dsi_device *dsi, u8 cmd, void *data,
+			  size_t len);
+int mipi_dsi_dcs_nop(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_soft_reset(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_get_power_mode(struct mipi_dsi_device *dsi, u8 *mode);
+int mipi_dsi_dcs_get_pixel_format(struct mipi_dsi_device *dsi, u8 *format);
+int mipi_dsi_dcs_enter_sleep_mode(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_exit_sleep_mode(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_set_display_off(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_set_display_on(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_set_column_address(struct mipi_dsi_device *dsi, u16 start,
+				    u16 end);
+int mipi_dsi_dcs_set_page_address(struct mipi_dsi_device *dsi, u16 start,
+				  u16 end);
+int mipi_dsi_dcs_set_tear_off(struct mipi_dsi_device *dsi);
+int mipi_dsi_dcs_set_tear_on(struct mipi_dsi_device *dsi,
+			     enum mipi_dsi_dcs_tear_mode mode);
+int mipi_dsi_dcs_set_pixel_format(struct mipi_dsi_device *dsi, u8 format);
+int mipi_dsi_dcs_set_tear_scanline(struct mipi_dsi_device *dsi, u16 scanline);
+int mipi_dsi_dcs_set_display_brightness(struct mipi_dsi_device *dsi,
+					u16 brightness);
+int mipi_dsi_dcs_get_display_brightness(struct mipi_dsi_device *dsi,
+					u16 *brightness);
+
 #endif
diff --git a/include/power/stpmic1.h b/include/power/stpmic1.h
new file mode 100644
index 0000000..d90a1a9
--- /dev/null
+++ b/include/power/stpmic1.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __PMIC_STPMIC1_H_
+#define __PMIC_STPMIC1_H_
+
+#define STPMIC1_MAIN_CR			0x10
+#define STPMIC1_BUCKS_MRST_CR		0x18
+#define STPMIC1_LDOS_MRST_CR		0x1a
+#define STPMIC1_BUCKX_MAIN_CR(buck)	(0x20 + (buck))
+#define STPMIC1_REFDDR_MAIN_CR		0x24
+#define STPMIC1_LDOX_MAIN_CR(ldo)	(0x25 + (ldo))
+#define STPMIC1_BST_SW_CR		0x40
+#define STPMIC1_NVM_SR			0xb8
+#define STPMIC1_NVM_CR			0xb9
+
+/* Main PMIC Control Register (MAIN_CR) */
+#define STPMIC1_SWOFF			BIT(0)
+#define STPMIC1_RREQ_EN			BIT(1)
+
+/* BUCKS_MRST_CR */
+#define STPMIC1_MRST_BUCK(buck)		BIT(buck)
+#define STPMIC1_MRST_BUCK_DEBUG		(STPMIC1_MRST_BUCK(STPMIC1_BUCK1) | \
+					 STPMIC1_MRST_BUCK(STPMIC1_BUCK3))
+
+/* LDOS_MRST_CR */
+#define STPMIC1_MRST_LDO(ldo)		BIT(ldo)
+#define STPMIC1_MRST_LDO_DEBUG		0
+
+/* BUCKx_MAIN_CR (x=1...4) */
+#define STPMIC1_BUCK_ENA		BIT(0)
+#define STPMIC1_BUCK_PREG_MODE		BIT(1)
+#define STPMIC1_BUCK_VOUT_MASK		GENMASK(7, 2)
+#define STPMIC1_BUCK_VOUT_SHIFT		2
+#define STPMIC1_BUCK_VOUT(sel)		(sel << STPMIC1_BUCK_VOUT_SHIFT)
+
+#define STPMIC1_BUCK2_1200000V		STPMIC1_BUCK_VOUT(24)
+#define STPMIC1_BUCK2_1350000V		STPMIC1_BUCK_VOUT(30)
+
+#define STPMIC1_BUCK3_1800000V		STPMIC1_BUCK_VOUT(39)
+
+/* REFDDR_MAIN_CR */
+#define STPMIC1_VREF_ENA		BIT(0)
+
+/* LDOX_MAIN_CR */
+#define STPMIC1_LDO_ENA			BIT(0)
+#define STPMIC1_LDO12356_VOUT_MASK	GENMASK(6, 2)
+#define STPMIC1_LDO12356_VOUT_SHIFT	2
+#define STPMIC1_LDO_VOUT(sel)		(sel << STPMIC1_LDO12356_VOUT_SHIFT)
+
+#define STPMIC1_LDO3_MODE		BIT(7)
+#define STPMIC1_LDO3_DDR_SEL		31
+#define STPMIC1_LDO3_1800000		STPMIC1_LDO_VOUT(9)
+
+#define STPMIC1_LDO4_UV			3300000
+
+/* BST_SW_CR */
+#define STPMIC1_BST_ON			BIT(0)
+#define STPMIC1_VBUSOTG_ON		BIT(1)
+#define STPMIC1_SWOUT_ON		BIT(2)
+#define STPMIC1_PWR_SW_ON		(STPMIC1_VBUSOTG_ON | STPMIC1_SWOUT_ON)
+
+/* NVM_SR */
+#define STPMIC1_NVM_BUSY		BIT(0)
+
+/* NVM_CR */
+#define STPMIC1_NVM_CMD_PROGRAM		1
+#define STPMIC1_NVM_CMD_READ		2
+
+/* Timeout */
+#define STPMIC1_DEFAULT_START_UP_DELAY_MS	1
+#define STPMIC1_DEFAULT_STOP_DELAY_MS		5
+#define STPMIC1_USB_BOOST_START_UP_DELAY_MS	10
+
+enum {
+	STPMIC1_BUCK1,
+	STPMIC1_BUCK2,
+	STPMIC1_BUCK3,
+	STPMIC1_BUCK4,
+	STPMIC1_MAX_BUCK,
+};
+
+enum {
+	STPMIC1_PREG_MODE_HP,
+	STPMIC1_PREG_MODE_LP,
+};
+
+enum {
+	STPMIC1_LDO1,
+	STPMIC1_LDO2,
+	STPMIC1_LDO3,
+	STPMIC1_LDO4,
+	STPMIC1_LDO5,
+	STPMIC1_LDO6,
+	STPMIC1_MAX_LDO,
+};
+
+enum {
+	STPMIC1_LDO_MODE_NORMAL,
+	STPMIC1_LDO_MODE_BYPASS,
+	STPMIC1_LDO_MODE_SINK_SOURCE,
+};
+
+enum {
+	STPMIC1_PWR_SW1,
+	STPMIC1_PWR_SW2,
+	STPMIC1_MAX_PWR_SW,
+};
+
+int stpmic1_shadow_read_byte(u8 addr, u8 *buf);
+int stpmic1_shadow_write_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_read_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_write_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_read_all(u8 *buf, int buf_len);
+int stpmic1_nvm_write_all(u8 *buf, int buf_len);
+#endif
diff --git a/include/power/stpmu1.h b/include/power/stpmu1.h
deleted file mode 100644
index 5906fbf..0000000
--- a/include/power/stpmu1.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#ifndef __PMIC_STPMU1_H_
-#define __PMIC_STPMU1_H_
-
-#define STPMU1_MASK_RESET_BUCK		0x18
-#define STPMU1_BUCKX_CTRL_REG(buck)	(0x20 + (buck))
-#define STPMU1_VREF_CTRL_REG		0x24
-#define STPMU1_LDOX_CTRL_REG(ldo)	(0x25 + (ldo))
-#define STPMU1_USB_CTRL_REG		0x40
-#define STPMU1_NVM_USER_STATUS_REG	0xb8
-#define STPMU1_NVM_USER_CONTROL_REG	0xb9
-
-#define STPMU1_MASK_RESET_BUCK3		BIT(2)
-
-#define STPMU1_BUCK_EN			BIT(0)
-#define STPMU1_BUCK_MODE		BIT(1)
-#define STPMU1_BUCK_OUTPUT_MASK		GENMASK(7, 2)
-#define STPMU1_BUCK_OUTPUT_SHIFT	2
-#define STPMU1_BUCK2_1200000V		(24 << STPMU1_BUCK_OUTPUT_SHIFT)
-#define STPMU1_BUCK2_1350000V		(30 << STPMU1_BUCK_OUTPUT_SHIFT)
-#define STPMU1_BUCK3_1800000V		(39 << STPMU1_BUCK_OUTPUT_SHIFT)
-
-#define STPMU1_VREF_EN			BIT(0)
-
-#define STPMU1_LDO_EN			BIT(0)
-#define STPMU1_LDO12356_OUTPUT_MASK	GENMASK(6, 2)
-#define STPMU1_LDO12356_OUTPUT_SHIFT	2
-#define STPMU1_LDO3_MODE		BIT(7)
-#define STPMU1_LDO3_DDR_SEL		31
-#define STPMU1_LDO3_1800000		(9 << STPMU1_LDO12356_OUTPUT_SHIFT)
-#define STPMU1_LDO4_UV			3300000
-
-#define STPMU1_USB_BOOST_EN		BIT(0)
-#define STPMU1_USB_PWR_SW_EN		GENMASK(2, 1)
-
-#define STPMU1_NVM_USER_CONTROL_PROGRAM	BIT(0)
-#define STPMU1_NVM_USER_CONTROL_READ	BIT(1)
-
-#define STPMU1_NVM_USER_STATUS_BUSY	BIT(0)
-#define STPMU1_NVM_USER_STATUS_ERROR	BIT(1)
-
-#define STPMU1_DEFAULT_START_UP_DELAY_MS	1
-#define STPMU1_DEFAULT_STOP_DELAY_MS		5
-#define STPMU1_USB_BOOST_START_UP_DELAY_MS	10
-
-enum {
-	STPMU1_BUCK1,
-	STPMU1_BUCK2,
-	STPMU1_BUCK3,
-	STPMU1_BUCK4,
-	STPMU1_MAX_BUCK,
-};
-
-enum {
-	STPMU1_BUCK_MODE_HP,
-	STPMU1_BUCK_MODE_LP,
-};
-
-enum {
-	STPMU1_LDO1,
-	STPMU1_LDO2,
-	STPMU1_LDO3,
-	STPMU1_LDO4,
-	STPMU1_LDO5,
-	STPMU1_LDO6,
-	STPMU1_MAX_LDO,
-};
-
-enum {
-	STPMU1_LDO_MODE_NORMAL,
-	STPMU1_LDO_MODE_BYPASS,
-	STPMU1_LDO_MODE_SINK_SOURCE,
-};
-
-enum {
-	STPMU1_PWR_SW1,
-	STPMU1_PWR_SW2,
-	STPMU1_MAX_PWR_SW,
-};
-
-#endif
diff --git a/include/remoteproc.h b/include/remoteproc.h
index a59dba8..fa82531 100644
--- a/include/remoteproc.h
+++ b/include/remoteproc.h
@@ -63,6 +63,8 @@ struct dm_rproc_uclass_pdata {
  *		Return 0 on success, 1 if not running, -ve on others errors
  * @ping:	Ping the remote device for basic communication check(optional)
  *		Return 0 on success, 1 if not responding, -ve on other errors
+ * @da_to_pa:   Return translated physical address (device address different
+ *              from physical address)
  */
 struct dm_rproc_ops {
 	int (*init)(struct udevice *dev);
@@ -72,6 +74,7 @@ struct dm_rproc_ops {
 	int (*reset)(struct udevice *dev);
 	int (*is_running)(struct udevice *dev);
 	int (*ping)(struct udevice *dev);
+	ulong (*da_to_pa)(struct udevice *dev, ulong da);
 };
 
 /* Accessor */
@@ -101,7 +104,7 @@ int rproc_dev_init(int id);
 bool rproc_is_initialized(void);
 
 /**
- * rproc_load() - load binary to a remote processor
+ * rproc_load() - load binary or elf to a remote processor
  * @id:		id of the remote processor
  * @addr:	address in memory where the binary image is located
  * @size:	size of the binary image
@@ -111,6 +114,19 @@ bool rproc_is_initialized(void);
 int rproc_load(int id, ulong addr, ulong size);
 
 /**
+ * rproc_get_rsc_table() - get resource table address from elf image
+ * @id:		id of the remote processor
+ * @addr:	address in memory where the image is located
+ * @size:	size of the image
+ * @rsc_addr:	resource table address (device address)
+ * @rsc_size:   resource table size
+ *
+ * Return: 0 if all ok, else appropriate error value.
+ */
+int rproc_load_rsc_table(int id, ulong addr, ulong size, ulong *rsc_addr,
+			 unsigned int *rsc_size);
+
+/**
  * rproc_start() - Start a remote processor
  * @id:		id of the remote processor
  *
@@ -166,6 +182,8 @@ static inline int rproc_stop(int id) { return -ENOSYS; }
 static inline int rproc_reset(int id) { return -ENOSYS; }
 static inline int rproc_ping(int id) { return -ENOSYS; }
 static inline int rproc_is_running(int id) { return -ENOSYS; }
+static int rproc_load_rsc_table(int id, ulong addr, ulong size, ulong *rsc_addr,
+				unsigned int *rsc_size) { return -ENOSYS; }
 #endif
 
 #endif	/* _RPROC_H_ */
diff --git a/include/syscon.h b/include/syscon.h
index 2aa73e5..1b99751 100644
--- a/include/syscon.h
+++ b/include/syscon.h
@@ -89,4 +89,13 @@ void *syscon_get_first_range(ulong driver_data);
  */
 struct regmap *syscon_node_to_regmap(ofnode node);
 
+/**
+ * syscon_phandle_to_regmap - get regmap from syscon phandle
+ *
+ * @parent: Parent device containing the phandle pointer
+ * @name: Name of property in the parent device node
+ */
+struct regmap *syscon_phandle_to_regmap(struct udevice *parent,
+					const char *name);
+
 #endif
diff --git a/include/usb/dwc2_udc.h b/include/usb/dwc2_udc.h
index 62e3236..3bd05fa 100644
--- a/include/usb/dwc2_udc.h
+++ b/include/usb/dwc2_udc.h
@@ -19,6 +19,7 @@ struct dwc2_plat_otg_data {
 	unsigned int    usb_phy_ctrl;
 	unsigned int    usb_flags;
 	unsigned int	usb_gusbcfg;
+	unsigned int	usb_gotgctl;
 	unsigned int	rx_fifo_sz;
 	unsigned int	np_tx_fifo_sz;
 	unsigned int	tx_fifo_sz;
diff --git a/test/dm/Makefile b/test/dm/Makefile
index b490cf2..9b3b0bf 100644
--- a/test/dm/Makefile
+++ b/test/dm/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_CLK) += clk.o
 obj-$(CONFIG_DM_ETH) += eth.o
 obj-$(CONFIG_FIRMWARE) += firmware.o
 obj-$(CONFIG_DM_GPIO) += gpio.o
+obj-$(CONFIG_DM_HWSPINLOCK) += hwspinlock.o
 obj-$(CONFIG_DM_I2C) += i2c.o
 obj-$(CONFIG_LED) += led.o
 obj-$(CONFIG_DM_MAILBOX) += mailbox.o
diff --git a/test/dm/hwspinlock.c b/test/dm/hwspinlock.c
new file mode 100644
index 0000000..09ec38b
--- /dev/null
+++ b/test/dm/hwspinlock.c
@@ -0,0 +1,40 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <hwspinlock.h>
+#include <asm/state.h>
+#include <asm/test.h>
+#include <dm/test.h>
+#include <test/ut.h>
+
+/* Test that hwspinlock driver functions are called */
+static int dm_test_hwspinlock_base(struct unit_test_state *uts)
+{
+	struct sandbox_state *state = state_get_current();
+	struct hwspinlock hws;
+
+	ut_assertok(uclass_get_device(UCLASS_HWSPINLOCK, 0, &hws.dev));
+	ut_assertnonnull(hws.dev);
+	ut_asserteq(false, state->hwspinlock);
+
+	hws.id = 0;
+	ut_assertok(hwspinlock_lock_timeout(&hws, 1));
+	ut_asserteq(true, state->hwspinlock);
+
+	ut_assertok(hwspinlock_unlock(&hws));
+	ut_asserteq(false, state->hwspinlock);
+
+	ut_assertok(hwspinlock_lock_timeout(&hws, 1));
+	ut_assertok(!hwspinlock_lock_timeout(&hws, 1));
+
+	ut_assertok(hwspinlock_unlock(&hws));
+	ut_assertok(!hwspinlock_unlock(&hws));
+
+	return 0;
+}
+
+DM_TEST(dm_test_hwspinlock_base, DM_TESTF_SCAN_PDATA | DM_TESTF_SCAN_FDT);
diff --git a/test/py/tests/test_pinmux.py b/test/py/tests/test_pinmux.py
new file mode 100644
index 0000000..f04a279
--- /dev/null
+++ b/test/py/tests/test_pinmux.py
@@ -0,0 +1,62 @@
+# SPDX-License-Identifier: GPL-2.0
+
+import pytest
+import u_boot_utils
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_usage_1(u_boot_console):
+    """Test that 'pinmux' command without parameters displays
+    pinmux usage."""
+    output = u_boot_console.run_command('pinmux')
+    assert 'Usage:' in output
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_usage_2(u_boot_console):
+    """Test that 'pinmux status' executed without previous "pinmux dev"
+    command displays pinmux usage."""
+    output = u_boot_console.run_command('pinmux status')
+    assert 'Usage:' in output
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_status_all(u_boot_console):
+    """Test that 'pinmux status -a' displays pin's muxing."""
+    output = u_boot_console.run_command('pinmux status -a')
+    assert ('SCL       : I2C SCL' in output)
+    assert ('SDA       : I2C SDA' in output)
+    assert ('TX        : Uart TX' in output)
+    assert ('RX        : Uart RX' in output)
+    assert ('W1        : 1-wire gpio' in output)
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_list(u_boot_console):
+    """Test that 'pinmux list' returns the pin-controller list."""
+    output = u_boot_console.run_command('pinmux list')
+    assert 'sandbox_pinctrl' in output
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_dev_bad(u_boot_console):
+    """Test that 'pinmux dev' returns an error when trying to select a
+    wrong pin controller."""
+    pincontroller = 'bad_pin_controller_name'
+    output = u_boot_console.run_command('pinmux dev ' + pincontroller)
+    expected_output = 'Can\'t get the pin-controller: ' + pincontroller + '!'
+    assert (expected_output in output)
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_dev(u_boot_console):
+    """Test that 'pinmux dev' select the wanted pin controller."""
+    pincontroller = 'pinctrl'
+    output = u_boot_console.run_command('pinmux dev ' + pincontroller)
+    expected_output = 'dev: ' + pincontroller
+    assert (expected_output in output)
+
+@pytest.mark.buildconfigspec('cmd_pinmux')
+def test_pinmux_status(u_boot_console):
+    """Test that 'pinmux status' displays selected pincontroller's pin
+    muxing descriptions."""
+    output = u_boot_console.run_command('pinmux status')
+    assert ('SCL       : I2C SCL' in output)
+    assert ('SDA       : I2C SDA' in output)
+    assert ('TX        : Uart TX' in output)
+    assert ('RX        : Uart RX' in output)
+    assert ('W1        : 1-wire gpio' in output)
diff --git a/tools/stm32image.c b/tools/stm32image.c
index 08b32ba..ff3ec5f 100644
--- a/tools/stm32image.c
+++ b/tools/stm32image.c
@@ -14,6 +14,8 @@
 #define HEADER_VERSION_V1	0x1
 /* default option : bit0 => no signature */
 #define HEADER_DEFAULT_OPTION	(cpu_to_le32(0x00000001))
+/* default binary type for U-Boot */
+#define HEADER_TYPE_UBOOT	(cpu_to_le32(0x00000000))
 
 struct stm32_header {
 	uint32_t magic_number;
@@ -29,7 +31,8 @@ struct stm32_header {
 	uint32_t option_flags;
 	uint32_t ecdsa_algorithm;
 	uint32_t ecdsa_public_key[64 / 4];
-	uint32_t padding[84 / 4];
+	uint32_t padding[83 / 4];
+	uint32_t binary_type;
 };
 
 static struct stm32_header stm32image_header;
@@ -43,6 +46,7 @@ static void stm32image_default_header(struct stm32_header *ptr)
 	ptr->header_version[VER_MAJOR_IDX] = HEADER_VERSION_V1;
 	ptr->option_flags = HEADER_DEFAULT_OPTION;
 	ptr->ecdsa_algorithm = 1;
+	ptr->binary_type = HEADER_TYPE_UBOOT;
 }
 
 static uint32_t stm32image_checksum(void *start, uint32_t len)
@@ -112,6 +116,8 @@ static void stm32image_print_header(const void *ptr)
 	       le32_to_cpu(stm32hdr->image_checksum));
 	printf("Option     : 0x%08x\n",
 	       le32_to_cpu(stm32hdr->option_flags));
+	printf("BinaryType : 0x%08x\n",
+	       le32_to_cpu(stm32hdr->binary_type));
 }
 
 static void stm32image_set_header(void *ptr, struct stat *sbuf, int ifd,
-- 
2.7.4

