Merge https://source.denx.de/u-boot/custodians/u-boot-riscv

- k210 updates
diff --git a/arch/arm/dts/stm32mp15-pinctrl.dtsi b/arch/arm/dts/stm32mp15-pinctrl.dtsi
index d3553e0..6161f59 100644
--- a/arch/arm/dts/stm32mp15-pinctrl.dtsi
+++ b/arch/arm/dts/stm32mp15-pinctrl.dtsi
@@ -1718,7 +1718,7 @@
 
 	stusb1600_pins_a: stusb1600-0 {
 		pins {
-			pinmux = <STM32_PINMUX('I', 11, ANALOG)>;
+			pinmux = <STM32_PINMUX('I', 11, GPIO)>;
 			bias-pull-up;
 		};
 	};
@@ -1737,20 +1737,20 @@
 	};
 
 	uart4_idle_pins_a: uart4-idle-0 {
-		   pins1 {
-			 pinmux = <STM32_PINMUX('G', 11, ANALOG)>; /* UART4_TX */
-		   };
-		   pins2 {
-			 pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
-			 bias-disable;
-		   };
+		pins1 {
+			pinmux = <STM32_PINMUX('G', 11, ANALOG)>; /* UART4_TX */
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
+			bias-disable;
+		};
 	};
 
 	uart4_sleep_pins_a: uart4-sleep-0 {
-		   pins {
+		pins {
 			pinmux = <STM32_PINMUX('G', 11, ANALOG)>, /* UART4_TX */
 				 <STM32_PINMUX('B', 2, ANALOG)>; /* UART4_RX */
-		    };
+		};
 	};
 
 	uart4_pins_b: uart4-1 {
@@ -1816,7 +1816,7 @@
 		};
 		pins2 {
 			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* UART7_RX */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
@@ -1826,7 +1826,7 @@
 		};
 		pins2 {
 			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* UART7_RX */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
@@ -1971,7 +1971,7 @@
 		pins2 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
 				 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
@@ -1988,7 +1988,7 @@
 		};
 		pins3 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>; /* USART3_RX */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
@@ -2012,7 +2012,7 @@
 		pins2 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
 				 <STM32_PINMUX('B', 13, AF7)>; /* USART3_CTS_NSS */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
@@ -2029,7 +2029,7 @@
 		};
 		pins3 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>; /* USART3_RX */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
diff --git a/arch/arm/dts/stm32mp157c-ev1.dts b/arch/arm/dts/stm32mp157c-ev1.dts
index 5c5b1dd..e222d2d 100644
--- a/arch/arm/dts/stm32mp157c-ev1.dts
+++ b/arch/arm/dts/stm32mp157c-ev1.dts
@@ -375,3 +375,25 @@
 &usbphyc {
 	status = "okay";
 };
+
+&usbphyc_port0 {
+	st,tune-hs-dc-level = <2>;
+	st,enable-fs-rftime-tuning;
+	st,enable-hs-rftime-reduction;
+	st,trim-hs-current = <15>;
+	st,trim-hs-impedance = <1>;
+	st,tune-squelch-level = <3>;
+	st,tune-hs-rx-offset = <2>;
+	st,no-lsfs-sc;
+};
+
+&usbphyc_port1 {
+	st,tune-hs-dc-level = <2>;
+	st,enable-fs-rftime-tuning;
+	st,enable-hs-rftime-reduction;
+	st,trim-hs-current = <15>;
+	st,trim-hs-impedance = <1>;
+	st,tune-squelch-level = <3>;
+	st,tune-hs-rx-offset = <2>;
+	st,no-lsfs-sc;
+};
diff --git a/arch/arm/dts/stm32mp15xx-dhcom-u-boot.dtsi b/arch/arm/dts/stm32mp15xx-dhcom-u-boot.dtsi
index f09f429..d73967a 100644
--- a/arch/arm/dts/stm32mp15xx-dhcom-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp15xx-dhcom-u-boot.dtsi
@@ -58,6 +58,7 @@
 
 &i2c4 {
 	u-boot,dm-pre-reloc;
+	u-boot,dm-spl;
 
 	eeprom0: eeprom@50 {
 	};
@@ -98,6 +99,11 @@
 
 &pmic {
 	u-boot,dm-pre-reloc;
+	u-boot,dm-spl;
+
+	regulators {
+		u-boot,dm-spl;
+	};
 };
 
 &flash0 {
@@ -288,3 +294,39 @@
 		bias-pull-up;
 	};
 };
+
+&reg11 {
+	u-boot,dm-spl;
+};
+
+&reg18 {
+	u-boot,dm-spl;
+};
+
+&usb33 {
+	u-boot,dm-spl;
+};
+
+&usbotg_hs_pins_a {
+	u-boot,dm-spl;
+};
+
+&usbotg_hs {
+	u-boot,dm-spl;
+};
+
+&usbphyc {
+	u-boot,dm-spl;
+};
+
+&usbphyc_port0 {
+	u-boot,dm-spl;
+};
+
+&usbphyc_port1 {
+	u-boot,dm-spl;
+};
+
+&vdd_usb {
+	u-boot,dm-spl;
+};
diff --git a/arch/arm/dts/stm32mp15xx-dhcor-avenger96-u-boot.dtsi b/arch/arm/dts/stm32mp15xx-dhcor-avenger96-u-boot.dtsi
index 6e6543b..5bed53e 100644
--- a/arch/arm/dts/stm32mp15xx-dhcor-avenger96-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp15xx-dhcor-avenger96-u-boot.dtsi
@@ -101,3 +101,7 @@
 	u-boot,force-b-session-valid;
 	hnp-srp-disable;
 };
+
+&vdd_io {
+	u-boot,dm-spl;
+};
diff --git a/arch/arm/dts/stm32mp15xx-dhcor-u-boot.dtsi b/arch/arm/dts/stm32mp15xx-dhcor-u-boot.dtsi
index 338b674..19f4221 100644
--- a/arch/arm/dts/stm32mp15xx-dhcor-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp15xx-dhcor-u-boot.dtsi
@@ -179,6 +179,14 @@
 	u-boot,dm-spl;
 };
 
+&usb33 {
+	u-boot,dm-spl;
+};
+
+&usbotg_hs_pins_a {
+	u-boot,dm-spl;
+};
+
 &usbotg_hs {
 	u-boot,dm-spl;
 };
@@ -195,10 +203,6 @@
 	u-boot,dm-spl;
 };
 
-&vdd_io {
-	u-boot,dm-spl;
-};
-
 &vdd_usb {
 	u-boot,dm-spl;
 };
diff --git a/arch/arm/dts/stm32mp15xx-dkx.dtsi b/arch/arm/dts/stm32mp15xx-dkx.dtsi
index 5502eec..f8130bf 100644
--- a/arch/arm/dts/stm32mp15xx-dkx.dtsi
+++ b/arch/arm/dts/stm32mp15xx-dkx.dtsi
@@ -702,10 +702,26 @@
 
 &usbphyc_port0 {
 	phy-supply = <&vdd_usb>;
+	st,tune-hs-dc-level = <2>;
+	st,enable-fs-rftime-tuning;
+	st,enable-hs-rftime-reduction;
+	st,trim-hs-current = <15>;
+	st,trim-hs-impedance = <1>;
+	st,tune-squelch-level = <3>;
+	st,tune-hs-rx-offset = <2>;
+	st,no-lsfs-sc;
 };
 
 &usbphyc_port1 {
 	phy-supply = <&vdd_usb>;
+	st,tune-hs-dc-level = <2>;
+	st,enable-fs-rftime-tuning;
+	st,enable-hs-rftime-reduction;
+	st,trim-hs-current = <15>;
+	st,trim-hs-impedance = <1>;
+	st,tune-squelch-level = <3>;
+	st,tune-hs-rx-offset = <2>;
+	st,no-lsfs-sc;
 };
 
 &vrefbuf {
diff --git a/arch/arm/mach-stm32mp/bsec.c b/arch/arm/mach-stm32mp/bsec.c
index 27d1829..506caa0 100644
--- a/arch/arm/mach-stm32mp/bsec.c
+++ b/arch/arm/mach-stm32mp/bsec.c
@@ -18,6 +18,7 @@
 #include <linux/iopoll.h>
 
 #define BSEC_OTP_MAX_VALUE		95
+#define BSEC_OTP_UPPER_START		32
 #define BSEC_TIMEOUT_US			10000
 
 /* BSEC REGISTER OFFSET (base relative) */
@@ -41,6 +42,7 @@
 /* BSEC_CONTROL Register */
 #define BSEC_READ			0x000
 #define BSEC_WRITE			0x100
+#define BSEC_LOCK			0x200
 
 /* LOCK Register */
 #define OTP_LOCK_MASK			0x1F
@@ -61,6 +63,11 @@
  */
 #define BSEC_LOCK_PROGRAM		0x04
 
+/*
+ * OTP status: bit 0 permanent lock
+ */
+#define BSEC_LOCK_PERM			BIT(0)
+
 /**
  * bsec_lock() - manage lock for each type SR/SP/SW
  * @address: address of bsec IP register
@@ -160,6 +167,7 @@
 
 /**
  * bsec_shadow_register() - copy safmen otp to bsec data
+ * @dev: bsec IP device
  * @base: base address of bsec IP
  * @otp: otp number (0 - BSEC_OTP_MAX_VALUE)
  * Return: 0 if no error
@@ -203,6 +211,7 @@
 
 /**
  * bsec_read_shadow() - read an otp data value from shadow
+ * @dev: bsec IP device
  * @base: base address of bsec IP
  * @val: read value
  * @otp: otp number (0 - BSEC_OTP_MAX_VALUE)
@@ -217,6 +226,7 @@
 
 /**
  * bsec_write_shadow() - write value in BSEC data register in shadow
+ * @dev: bsec IP device
  * @base: base address of bsec IP
  * @val: value to write
  * @otp: otp number (0 - BSEC_OTP_MAX_VALUE)
@@ -235,6 +245,7 @@
 
 /**
  * bsec_program_otp() - program a bit in SAFMEM
+ * @dev: bsec IP device
  * @base: base address of bsec IP
  * @val: value to program
  * @otp: otp number (0 - BSEC_OTP_MAX_VALUE)
@@ -284,6 +295,65 @@
 	return ret;
 }
 
+/**
+ * bsec_permanent_lock_otp() - permanent lock of OTP in SAFMEM
+ * @dev: bsec IP device
+ * @base: base address of bsec IP
+ * @otp: otp number (0 - BSEC_OTP_MAX_VALUE)
+ * Return: 0 if no error
+ */
+static int bsec_permanent_lock_otp(struct udevice *dev, long base, uint32_t otp)
+{
+	int ret;
+	bool power_up = false;
+	u32 val, addr;
+
+	/* check if safemem is power up */
+	if (!(readl(base + BSEC_OTP_STATUS_OFF) & BSEC_MODE_PWR_MASK)) {
+		ret = bsec_power_safmem(base, true);
+		if (ret)
+			return ret;
+
+		power_up = true;
+	}
+
+	/*
+	 * low OTPs = 2 bits word for low OTPs, 1 bits per word for upper OTP
+	 * and only 16 bits used in WRDATA
+	 */
+	if (otp < BSEC_OTP_UPPER_START) {
+		addr = otp / 8;
+		val = 0x03 << ((otp * 2) & 0xF);
+	} else {
+		addr = BSEC_OTP_UPPER_START / 8 +
+		       ((otp - BSEC_OTP_UPPER_START) / 16);
+		val = 0x01 << (otp & 0xF);
+	}
+
+	/* set value in write register*/
+	writel(val, base + BSEC_OTP_WRDATA_OFF);
+
+	/* set BSEC_OTP_CTRL_OFF with the otp addr and lock request*/
+	writel(addr | BSEC_WRITE | BSEC_LOCK, base + BSEC_OTP_CTRL_OFF);
+
+	/* check otp status*/
+	ret = readl_poll_timeout(base + BSEC_OTP_STATUS_OFF,
+				 val, (val & BSEC_MODE_BUSY_MASK) == 0,
+				 BSEC_TIMEOUT_US);
+	if (ret)
+		return ret;
+
+	if (val & BSEC_MODE_PROGFAIL_MASK)
+		ret = -EACCES;
+	else
+		ret = bsec_check_error(base, otp);
+
+	if (power_up)
+		bsec_power_safmem(base, false);
+
+	return ret;
+}
+
 /* BSEC MISC driver *******************************************************/
 struct stm32mp_bsec_plat {
 	u32 base;
@@ -339,9 +409,14 @@
 static int stm32mp_bsec_read_lock(struct udevice *dev, u32 *val, u32 otp)
 {
 	struct stm32mp_bsec_plat *plat = dev_get_plat(dev);
+	u32 wrlock;
 
 	/* return OTP permanent write lock status */
-	*val = bsec_read_lock(plat->base + BSEC_WRLOCK_OFF, otp);
+	wrlock = bsec_read_lock(plat->base + BSEC_WRLOCK_OFF, otp);
+
+	*val = 0;
+	if (wrlock)
+		*val = BSEC_LOCK_PERM;
 
 	return 0;
 }
@@ -377,15 +452,22 @@
 
 static int stm32mp_bsec_write_lock(struct udevice *dev, u32 val, u32 otp)
 {
-	if (!IS_ENABLED(CONFIG_ARM_SMCCC) || IS_ENABLED(CONFIG_SPL_BUILD))
-		return -ENOTSUPP;
+	struct stm32mp_bsec_plat *plat;
 
-	if (val == 1)
+	/* only permanent write lock is supported in U-Boot */
+	if (!(val & BSEC_LOCK_PERM)) {
+		dev_dbg(dev, "lock option without BSEC_LOCK_PERM: %x\n", val);
+		return 0; /* nothing to do */
+	}
+
+	if (IS_ENABLED(CONFIG_ARM_SMCCC) && !IS_ENABLED(CONFIG_SPL_BUILD))
 		return stm32_smc_exec(STM32_SMC_BSEC,
 				      STM32_SMC_WRLOCK_OTP,
 				      otp, 0);
-	if (val == 0)
-		return 0; /* nothing to do */
+
+	plat = dev_get_plat(dev);
+
+	return bsec_permanent_lock_otp(dev, plat->base, otp);
 
 	return -EINVAL;
 }
diff --git a/arch/arm/mach-stm32mp/include/mach/stm32.h b/arch/arm/mach-stm32mp/include/mach/stm32.h
index c11a990..47e88fc 100644
--- a/arch/arm/mach-stm32mp/include/mach/stm32.h
+++ b/arch/arm/mach-stm32mp/include/mach/stm32.h
@@ -16,8 +16,11 @@
  */
 #define STM32_RCC_BASE			0x50000000
 #define STM32_PWR_BASE			0x50001000
+#define STM32_SYSCFG_BASE		0x50020000
 #define STM32_DBGMCU_BASE		0x50081000
 #define STM32_FMC2_BASE			0x58002000
+#define STM32_DDRCTRL_BASE		0x5A003000
+#define STM32_DDRPHYC_BASE		0x5A004000
 #define STM32_TZC_BASE			0x5C006000
 #define STM32_ETZPC_BASE		0x5C007000
 #define STM32_STGEN_BASE		0x5C008000
diff --git a/arch/arm/mach-stm32mp/psci.c b/arch/arm/mach-stm32mp/psci.c
index 155aa79..86c1609 100644
--- a/arch/arm/mach-stm32mp/psci.c
+++ b/arch/arm/mach-stm32mp/psci.c
@@ -11,19 +11,152 @@
 #include <asm/io.h>
 #include <asm/psci.h>
 #include <asm/secure.h>
+#include <hang.h>
 #include <linux/bitops.h>
 
-#define BOOT_API_A7_CORE0_MAGIC_NUMBER	0xCA7FACE0
-#define BOOT_API_A7_CORE1_MAGIC_NUMBER	0xCA7FACE1
+/* PWR */
+#define PWR_CR3					0x0c
+#define PWR_MPUCR				0x10
 
-#define MPIDR_AFF0			GENMASK(7, 0)
+#define PWR_CR3_DDRSREN				BIT(10)
+#define PWR_CR3_DDRRETEN			BIT(12)
 
-#define RCC_MP_GRSTCSETR		(STM32_RCC_BASE + 0x0404)
-#define RCC_MP_GRSTCSETR_MPUP1RST	BIT(5)
-#define RCC_MP_GRSTCSETR_MPUP0RST	BIT(4)
-#define RCC_MP_GRSTCSETR_MPSYSRST	BIT(0)
+#define PWR_MPUCR_PDDS				BIT(0)
+#define PWR_MPUCR_CSTDBYDIS			BIT(3)
+#define PWR_MPUCR_CSSF				BIT(9)
 
-#define STM32MP1_PSCI_NR_CPUS		2
+/* RCC */
+#define RCC_DDRITFCR				0xd8
+
+#define RCC_DDRITFCR_DDRC1EN			BIT(0)
+#define RCC_DDRITFCR_DDRC1LPEN			BIT(1)
+#define RCC_DDRITFCR_DDRC2EN			BIT(2)
+#define RCC_DDRITFCR_DDRC2LPEN			BIT(3)
+#define RCC_DDRITFCR_DDRPHYCEN			BIT(4)
+#define RCC_DDRITFCR_DDRPHYCLPEN		BIT(5)
+#define RCC_DDRITFCR_DDRCAPBEN			BIT(6)
+#define RCC_DDRITFCR_DDRCAPBLPEN		BIT(7)
+#define RCC_DDRITFCR_AXIDCGEN			BIT(8)
+#define RCC_DDRITFCR_DDRPHYCAPBEN		BIT(9)
+#define RCC_DDRITFCR_DDRPHYCAPBLPEN		BIT(10)
+#define RCC_DDRITFCR_DDRCKMOD_MASK		GENMASK(22, 20)
+#define RCC_DDRITFCR_GSKPCTRL			BIT(24)
+
+#define RCC_MP_SREQSETR				0x104
+#define RCC_MP_SREQCLRR				0x108
+
+#define RCC_MP_CIER				0x414
+#define RCC_MP_CIFR				0x418
+#define RCC_MP_CIFR_WKUPF			BIT(20)
+
+/* SYSCFG */
+#define SYSCFG_CMPCR				0x20
+#define SYSCFG_CMPCR_SW_CTRL			BIT(2)
+#define SYSCFG_CMPENSETR			0x24
+#define SYSCFG_CMPENCLRR			0x28
+#define SYSCFG_CMPENR_MPUEN			BIT(0)
+
+/* DDR Controller registers offsets */
+#define DDRCTRL_STAT				0x004
+#define DDRCTRL_PWRCTL				0x030
+#define DDRCTRL_PWRTMG				0x034
+#define DDRCTRL_HWLPCTL				0x038
+#define DDRCTRL_DFIMISC				0x1b0
+#define DDRCTRL_SWCTL				0x320
+#define DDRCTRL_SWSTAT				0x324
+#define DDRCTRL_PSTAT				0x3fc
+#define DDRCTRL_PCTRL_0				0x490
+#define DDRCTRL_PCTRL_1				0x540
+
+/* DDR Controller Register fields */
+#define DDRCTRL_STAT_OPERATING_MODE_MASK	GENMASK(2, 0)
+#define DDRCTRL_STAT_OPERATING_MODE_NORMAL	0x1
+#define DDRCTRL_STAT_OPERATING_MODE_SR		0x3
+#define DDRCTRL_STAT_SELFREF_TYPE_MASK		GENMASK(5, 4)
+#define DDRCTRL_STAT_SELFREF_TYPE_ASR		(0x3 << 4)
+#define DDRCTRL_STAT_SELFREF_TYPE_SR		(0x2 << 4)
+
+#define DDRCTRL_PWRCTL_SELFREF_EN		BIT(0)
+#define DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE	BIT(3)
+#define DDRCTRL_PWRCTL_SELFREF_SW		BIT(5)
+
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK	GENMASK(23, 16)
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_0		BIT(16)
+
+#define DDRCTRL_HWLPCTL_HW_LP_EN		BIT(0)
+
+#define DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN	BIT(0)
+
+#define DDRCTRL_SWCTL_SW_DONE			BIT(0)
+
+#define DDRCTRL_SWSTAT_SW_DONE_ACK		BIT(0)
+
+#define DDRCTRL_PSTAT_RD_PORT_BUSY_0		BIT(0)
+#define DDRCTRL_PSTAT_RD_PORT_BUSY_1		BIT(1)
+#define DDRCTRL_PSTAT_WR_PORT_BUSY_0		BIT(16)
+#define DDRCTRL_PSTAT_WR_PORT_BUSY_1		BIT(17)
+
+#define DDRCTRL_PCTRL_N_PORT_EN			BIT(0)
+
+/* DDR PHY registers offsets */
+#define DDRPHYC_PIR				0x004
+#define DDRPHYC_PGSR				0x00c
+#define DDRPHYC_ACDLLCR				0x014
+#define DDRPHYC_ACIOCR				0x024
+#define DDRPHYC_DXCCR				0x028
+#define DDRPHYC_DSGCR				0x02c
+#define DDRPHYC_ZQ0CR0				0x180
+#define DDRPHYC_DX0DLLCR			0x1cc
+#define DDRPHYC_DX1DLLCR			0x20c
+#define DDRPHYC_DX2DLLCR			0x24c
+#define DDRPHYC_DX3DLLCR			0x28c
+
+/* DDR PHY Register fields */
+#define DDRPHYC_PIR_INIT			BIT(0)
+#define DDRPHYC_PIR_DLLSRST			BIT(1)
+#define DDRPHYC_PIR_DLLLOCK			BIT(2)
+#define DDRPHYC_PIR_ITMSRST			BIT(4)
+
+#define DDRPHYC_PGSR_IDONE			BIT(0)
+
+#define DDRPHYC_ACDLLCR_DLLSRST			BIT(30)
+#define DDRPHYC_ACDLLCR_DLLDIS			BIT(31)
+
+#define DDRPHYC_ACIOCR_ACOE			BIT(1)
+#define DDRPHYC_ACIOCR_ACPDD			BIT(3)
+#define DDRPHYC_ACIOCR_ACPDR			BIT(4)
+#define DDRPHYC_ACIOCR_CKPDD_MASK		GENMASK(10, 8)
+#define DDRPHYC_ACIOCR_CKPDD_0			BIT(8)
+#define DDRPHYC_ACIOCR_CKPDR_MASK		GENMASK(13, 11)
+#define DDRPHYC_ACIOCR_CKPDR_0			BIT(11)
+#define DDRPHYC_ACIOCR_CSPDD_MASK		GENMASK(20, 18)
+#define DDRPHYC_ACIOCR_CSPDD_0			BIT(18)
+
+#define DDRPHYC_DXCCR_DXPDD			BIT(2)
+#define DDRPHYC_DXCCR_DXPDR			BIT(3)
+
+#define DDRPHYC_DSGCR_CKEPDD_MASK		GENMASK(19, 16)
+#define DDRPHYC_DSGCR_CKEPDD_0			BIT(16)
+#define DDRPHYC_DSGCR_ODTPDD_MASK		GENMASK(23, 20)
+#define DDRPHYC_DSGCR_ODTPDD_0			BIT(20)
+#define DDRPHYC_DSGCR_NL2PD			BIT(24)
+#define DDRPHYC_DSGCR_CKOE			BIT(28)
+
+#define DDRPHYC_ZQ0CRN_ZQPD			BIT(31)
+
+#define DDRPHYC_DXNDLLCR_DLLDIS			BIT(31)
+
+#define BOOT_API_A7_CORE0_MAGIC_NUMBER		0xca7face0
+#define BOOT_API_A7_CORE1_MAGIC_NUMBER		0xca7face1
+
+#define MPIDR_AFF0				GENMASK(7, 0)
+
+#define RCC_MP_GRSTCSETR			(STM32_RCC_BASE + 0x0404)
+#define RCC_MP_GRSTCSETR_MPSYSRST		BIT(0)
+#define RCC_MP_GRSTCSETR_MPUP0RST		BIT(4)
+#define RCC_MP_GRSTCSETR_MPUP1RST		BIT(5)
+
+#define STM32MP1_PSCI_NR_CPUS			2
 #if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
 #error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
 #endif
@@ -98,6 +231,7 @@
 	case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
 	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
 	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
+	case ARM_PSCI_1_0_FN_SYSTEM_SUSPEND:
 		return 0x0;
 	}
 	return ARM_PSCI_RET_NI;
@@ -222,3 +356,374 @@
 	while (1)
 		wfi();
 }
+
+static void __secure secure_udelay(unsigned int delay)
+{
+	u32 freq = cp15_read_cntfrq() / 1000000;
+	u64 start, end;
+
+	delay *= freq;
+
+	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start));
+	for (;;) {
+		asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end));
+		if ((end - start) > delay)
+			break;
+	}
+}
+
+static int __secure secure_waitbits(u32 reg, u32 mask, u32 val)
+{
+	u32 freq = cp15_read_cntfrq() / 1000000;
+	u32 delay = 500 * freq;	/* 500 us */
+	u64 start, end;
+	u32 tmp;
+
+	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start));
+	for (;;) {
+		tmp = readl(reg);
+		tmp &= mask;
+		if ((tmp & val) == val)
+			return 0;
+		asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end));
+		if ((end - start) > delay)
+			return -ETIMEDOUT;
+	}
+}
+
+static void __secure ddr_sr_mode_ssr(u32 *saved_pwrctl)
+{
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1LPEN | RCC_DDRITFCR_DDRC1EN |
+		     RCC_DDRITFCR_DDRC2LPEN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRCAPBLPEN | RCC_DDRITFCR_DDRPHYCAPBLPEN |
+		     RCC_DDRITFCR_DDRCAPBEN | RCC_DDRITFCR_DDRPHYCAPBEN |
+		     RCC_DDRITFCR_DDRPHYCEN);
+
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_AXIDCGEN | RCC_DDRITFCR_DDRCKMOD_MASK);
+
+	/* Disable HW LP interface of uMCTL2 */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_HWLPCTL,
+		     DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	clrsetbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRTMG,
+			DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/* Save PWRCTL register to restart ASR after suspend (if applicable) */
+	*saved_pwrctl = readl(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL);
+
+	/*
+	 * Disable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Disable automatic Self-Refresh mode */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void __secure ddr_sr_mode_restore(u32 saved_pwrctl)
+{
+	saved_pwrctl &= DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE |
+			DDRCTRL_PWRCTL_SELFREF_EN;
+
+	/* Restore ASR mode in case it was enabled before suspend. */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, saved_pwrctl);
+}
+
+static int __secure ddr_sw_self_refresh_in(void)
+{
+	int ret;
+
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	/* Blocks AXI ports from taking anymore transactions */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+
+	/*
+	 * Waits unit all AXI ports are idle
+	 * Poll PSTAT.rd_port_busy_n = 0
+	 * Poll PSTAT.wr_port_busy_n = 0
+	 */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_PSTAT,
+			      DDRCTRL_PSTAT_RD_PORT_BUSY_0 |
+			      DDRCTRL_PSTAT_RD_PORT_BUSY_1 |
+			      DDRCTRL_PSTAT_WR_PORT_BUSY_0 |
+			      DDRCTRL_PSTAT_WR_PORT_BUSY_1, 0);
+	if (ret)
+		goto pstat_failed;
+
+	/* SW Self-Refresh entry */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/*
+	 * Wait operating mode change in self-refresh mode
+	 * with STAT.operating_mode[1:0]==11.
+	 * Ensure transition to self-refresh was due to software
+	 * by checking also that STAT.selfref_type[1:0]=2.
+	 */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_STAT,
+			      DDRCTRL_STAT_OPERATING_MODE_MASK |
+			      DDRCTRL_STAT_SELFREF_TYPE_MASK,
+			      DDRCTRL_STAT_OPERATING_MODE_SR |
+			      DDRCTRL_STAT_SELFREF_TYPE_SR);
+	if (ret)
+		goto selfref_sw_failed;
+
+	/* IOs powering down (PUBL registers) */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDR);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDD_MASK,
+			DDRPHYC_ACIOCR_CKPDD_0);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDR_MASK,
+			DDRPHYC_ACIOCR_CKPDR_0);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CSPDD_MASK,
+			DDRPHYC_ACIOCR_CSPDD_0);
+
+	/* Disable command/address output driver */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_ODTPDD_MASK,
+			DDRPHYC_DSGCR_ODTPDD_0);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_CKEPDD_MASK,
+			DDRPHYC_DSGCR_CKEPDD_0);
+
+	/* Disable PZQ cell (PUBL register) */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Set latch */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	/* Additional delay to avoid early latch */
+	secure_udelay(10);
+
+	/* Activate sw retention in PWRCTRL */
+	setbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRRETEN);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Disable all DLLs: GLITCH window */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL output clock */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Deactivate all DDR clocks */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1EN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRCAPBEN | RCC_DDRITFCR_DDRPHYCAPBEN);
+
+	return 0;
+
+selfref_sw_failed:
+	/* This bit should be cleared to restore DDR in its previous state */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_SW);
+
+pstat_failed:
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+
+	return -EINVAL;
+};
+
+static void __secure ddr_sw_self_refresh_exit(void)
+{
+	int ret;
+
+	/* Enable all clocks */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1EN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRPHYCEN | RCC_DDRITFCR_DDRPHYCAPBEN |
+		     RCC_DDRITFCR_DDRCAPBEN);
+
+	/* Handshake */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	/* Mask dfi_init_complete_en */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_DFIMISC,
+		     DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	/* Ack */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_SWSTAT,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK);
+	if (ret)
+		hang();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Enable all DLLs: GLITCH window */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR,
+		     DDRPHYC_ACDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Additional delay to avoid early DLL clock switch */
+	secure_udelay(50);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	secure_udelay(10);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	/* PHY partial init: (DLL lock and ITM reset) */
+	writel(DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK |
+	       DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_INIT,
+	       STM32_DDRPHYC_BASE + DDRPHYC_PIR);
+
+	/* Need to wait at least 10 clock cycles before accessing PGSR */
+	secure_udelay(1);
+
+	/* Pool end of init */
+	ret = secure_waitbits(STM32_DDRPHYC_BASE + DDRPHYC_PGSR,
+			      DDRPHYC_PGSR_IDONE, DDRPHYC_PGSR_IDONE);
+	if (ret)
+		hang();
+
+	/* Handshake */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	/* Unmask dfi_init_complete_en to uMCTL2 */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_DFIMISC, DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	/* Ack */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_SWSTAT,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK);
+	if (ret)
+		hang();
+
+	/* Deactivate sw retention in PWR */
+	clrbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRRETEN);
+
+	/* Enable PZQ cell (PUBL register) */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Enable pad drivers */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	/* Enable command/address output driver */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CKPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CSPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	/* Release latch */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_ODTPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKEPDD_MASK);
+
+	/* Remove selfrefresh */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating_mode == normal */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_STAT,
+			      DDRCTRL_STAT_OPERATING_MODE_MASK,
+			      DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+	if (ret)
+		hang();
+
+	/* AXI ports are no longer blocked from taking transactions */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0, DDRCTRL_PCTRL_N_PORT_EN);
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1, DDRCTRL_PCTRL_N_PORT_EN);
+
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+}
+
+void __secure psci_system_suspend(u32 __always_unused function_id,
+				  u32 ep, u32 context_id)
+{
+	u32 saved_pwrctl, reg;
+
+	/* Disable IO compensation */
+
+	/* Place current APSRC/ANSRC into RAPSRC/RANSRC */
+	reg = readl(STM32_SYSCFG_BASE + SYSCFG_CMPCR);
+	reg >>= 8;
+	reg &= 0xff << 16;
+	reg |= SYSCFG_CMPCR_SW_CTRL;
+	writel(reg, STM32_SYSCFG_BASE + SYSCFG_CMPCR);
+	writel(SYSCFG_CMPENR_MPUEN, STM32_SYSCFG_BASE + SYSCFG_CMPENCLRR);
+
+	writel(RCC_MP_CIFR_WKUPF, STM32_RCC_BASE + RCC_MP_CIFR);
+	setbits_le32(STM32_RCC_BASE + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	setbits_le32(STM32_PWR_BASE + PWR_MPUCR,
+		     PWR_MPUCR_CSSF | PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_PDDS);
+
+	psci_v7_flush_dcache_all();
+	ddr_sr_mode_ssr(&saved_pwrctl);
+	ddr_sw_self_refresh_in();
+	setbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRSREN);
+	writel(0x3, STM32_RCC_BASE + RCC_MP_SREQSETR);
+
+	/* Zzz, enter stop mode */
+	asm volatile(
+		"isb\n"
+		"dsb\n"
+		"wfi\n");
+
+	writel(0x3, STM32_RCC_BASE + RCC_MP_SREQCLRR);
+	ddr_sw_self_refresh_exit();
+	ddr_sr_mode_restore(saved_pwrctl);
+
+	writel(SYSCFG_CMPENR_MPUEN, STM32_SYSCFG_BASE + SYSCFG_CMPENSETR);
+	clrbits_le32(STM32_SYSCFG_BASE + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
+}
diff --git a/board/nokia/rx51/rx51.c b/board/nokia/rx51/rx51.c
index a526915..621cff0 100644
--- a/board/nokia/rx51/rx51.c
+++ b/board/nokia/rx51/rx51.c
@@ -30,7 +30,7 @@
 #include <malloc.h>
 #include <twl4030.h>
 #include <i2c.h>
-#include <video_fb.h>
+#include <video.h>
 #include <keyboard.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
@@ -62,8 +62,6 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-GraphicDevice gdev;
-
 const omap3_sysinfo sysinfo = {
 	DDR_STACKED,
 	"Nokia RX-51",
@@ -342,22 +340,28 @@
 	*in_params = params;
 }
 
-/*
- * Routine: video_hw_init
- * Description: Set up the GraphicDevice depending on sys_boot.
- */
-void *video_hw_init(void)
+static int rx51_video_probe(struct udevice *dev)
 {
-	/* fill in Graphic Device */
-	gdev.frameAdrs = 0x8f9c0000;
-	gdev.winSizeX = 800;
-	gdev.winSizeY = 480;
-	gdev.gdfBytesPP = 2;
-	gdev.gdfIndex = GDF_16BIT_565RGB;
-	memset((void *)gdev.frameAdrs, 0, 0xbb800);
-	return (void *) &gdev;
+	struct video_uc_plat *uc_plat = dev_get_uclass_plat(dev);
+	struct video_priv *uc_priv = dev_get_uclass_priv(dev);
+
+	uc_plat->base = 0x8f9c0000;
+	uc_plat->size = 800 * 480 * sizeof(u16);
+	uc_priv->xsize = 800;
+	uc_priv->ysize = 480;
+	uc_priv->bpix = VIDEO_BPP16;
+
+	video_set_flush_dcache(dev, true);
+
+	return 0;
 }
 
+U_BOOT_DRIVER(rx51_video) = {
+	.name = "rx51_video",
+	.id = UCLASS_VIDEO,
+	.probe = rx51_video_probe,
+};
+
 /*
  * Routine: twl4030_regulator_set_mode
  * Description: Set twl4030 regulator mode over i2c powerbus.
@@ -777,6 +781,10 @@
 	{ "rx51_watchdog" },
 };
 
+U_BOOT_DRVINFOS(rx51_video) = {
+	{ "rx51_video" },
+};
+
 U_BOOT_DRVINFOS(rx51_kp) = {
 	{ "rx51_kp" },
 };
diff --git a/board/st/common/cmd_stboard.c b/board/st/common/cmd_stboard.c
index 2fba383..c1ecd64 100644
--- a/board/st/common/cmd_stboard.c
+++ b/board/st/common/cmd_stboard.c
@@ -91,14 +91,14 @@
 	ret = misc_read(dev, STM32_BSEC_OTP(BSEC_OTP_BOARD),
 			&otp, sizeof(otp));
 
-	if (ret < 0) {
+	if (ret != sizeof(otp)) {
 		puts("OTP read error");
 		return CMD_RET_FAILURE;
 	}
 
 	ret = misc_read(dev, STM32_BSEC_LOCK(BSEC_OTP_BOARD),
 			&lock, sizeof(lock));
-	if (ret < 0) {
+	if (ret != sizeof(lock)) {
 		puts("LOCK read error");
 		return CMD_RET_FAILURE;
 	}
@@ -172,7 +172,7 @@
 	ret = misc_write(dev, STM32_BSEC_OTP(BSEC_OTP_BOARD),
 			 &otp, sizeof(otp));
 
-	if (ret < 0) {
+	if (ret != sizeof(otp)) {
 		puts("BOARD programming error\n");
 		return CMD_RET_FAILURE;
 	}
@@ -181,7 +181,7 @@
 	otp = 1;
 	ret = misc_write(dev, STM32_BSEC_LOCK(BSEC_OTP_BOARD),
 			 &otp, sizeof(otp));
-	if (ret < 0) {
+	if (ret != sizeof(otp)) {
 		puts("BOARD lock error\n");
 		return CMD_RET_FAILURE;
 	}
diff --git a/configs/nokia_rx51_defconfig b/configs/nokia_rx51_defconfig
index 47b7bc3..1d64981 100644
--- a/configs/nokia_rx51_defconfig
+++ b/configs/nokia_rx51_defconfig
@@ -77,8 +77,11 @@
 CONFIG_USB=y
 CONFIG_USB_MUSB_UDC=y
 CONFIG_USB_OMAP3=y
-CONFIG_CFB_CONSOLE=y
-CONFIG_CFB_CONSOLE_ANSI=y
+CONFIG_DM_VIDEO=y
+CONFIG_VIDEO_LOGO=y
+# CONFIG_VIDEO_BPP8 is not set
+# CONFIG_VIDEO_BPP32 is not set
+CONFIG_SYS_WHITE_ON_BLACK=y
 CONFIG_SPLASH_SCREEN=y
 CONFIG_WATCHDOG_TIMEOUT_MSECS=31000
 CONFIG_WDT=y
diff --git a/configs/r8a77970_eagle_defconfig b/configs/r8a77970_eagle_defconfig
index 70b23da..4ccc6f1 100644
--- a/configs/r8a77970_eagle_defconfig
+++ b/configs/r8a77970_eagle_defconfig
@@ -51,7 +51,7 @@
 CONFIG_DFU_SF=y
 CONFIG_RCAR_GPIO=y
 CONFIG_DM_I2C=y
-CONFIG_SYS_I2C_RCAR_IIC=y
+CONFIG_SYS_I2C_RCAR_I2C=y
 # CONFIG_MMC is not set
 CONFIG_MTD=y
 CONFIG_DM_SPI_FLASH=y
diff --git a/configs/r8a77980_condor_defconfig b/configs/r8a77980_condor_defconfig
index 194fdde..36b39c1 100644
--- a/configs/r8a77980_condor_defconfig
+++ b/configs/r8a77980_condor_defconfig
@@ -52,7 +52,7 @@
 CONFIG_DFU_SF=y
 CONFIG_RCAR_GPIO=y
 CONFIG_DM_I2C=y
-CONFIG_SYS_I2C_RCAR_IIC=y
+CONFIG_SYS_I2C_RCAR_I2C=y
 # CONFIG_MMC is not set
 CONFIG_MTD=y
 CONFIG_DM_SPI_FLASH=y
diff --git a/configs/r8a779a0_falcon_defconfig b/configs/r8a779a0_falcon_defconfig
index 32e218b..9fce6c2 100644
--- a/configs/r8a779a0_falcon_defconfig
+++ b/configs/r8a779a0_falcon_defconfig
@@ -47,7 +47,7 @@
 CONFIG_CLK_RENESAS=y
 CONFIG_RCAR_GPIO=y
 CONFIG_DM_I2C=y
-CONFIG_SYS_I2C_RCAR_IIC=y
+CONFIG_SYS_I2C_RCAR_I2C=y
 CONFIG_MMC_IO_VOLTAGE=y
 CONFIG_MMC_UHS_SUPPORT=y
 CONFIG_MMC_HS200_SUPPORT=y
diff --git a/configs/stm32mp15_dhcom_basic_defconfig b/configs/stm32mp15_dhcom_basic_defconfig
index c422c47..438bba3 100644
--- a/configs/stm32mp15_dhcom_basic_defconfig
+++ b/configs/stm32mp15_dhcom_basic_defconfig
@@ -18,6 +18,7 @@
 CONFIG_SYS_LOAD_ADDR=0xc2000000
 CONFIG_FIT=y
 CONFIG_SPL_LOAD_FIT=y
+CONFIG_SPL_LOAD_FIT_ADDRESS=0xc1000000
 CONFIG_SPL_FIT_SOURCE="board/dhelectronics/dh_stm32mp1/u-boot-dhcom.its"
 # CONFIG_USE_SPL_FIT_GENERATOR is not set
 CONFIG_BOOTDELAY=1
@@ -27,12 +28,17 @@
 CONFIG_SPL_LEGACY_IMAGE_SUPPORT=y
 CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_USE_PARTITION=y
 CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION=3
+CONFIG_SPL_ENV_SUPPORT=y
 CONFIG_SPL_I2C=y
 CONFIG_SPL_MTD_SUPPORT=y
 CONFIG_SPL_DM_SPI_FLASH=y
 CONFIG_SPL_POWER=y
+CONFIG_SPL_RAM_SUPPORT=y
+CONFIG_SPL_RAM_DEVICE=y
 CONFIG_SPL_SPI_FLASH_MTD=y
 CONFIG_SYS_SPI_U_BOOT_OFFS=0x80000
+CONFIG_SPL_USB_GADGET=y
+CONFIG_SPL_DFU=y
 CONFIG_SYS_PROMPT="STM32MP> "
 # CONFIG_CMD_ELF is not set
 # CONFIG_CMD_EXPORTENV is not set
@@ -71,6 +77,7 @@
 CONFIG_ENV_IS_IN_SPI_FLASH=y
 CONFIG_SYS_REDUNDAND_ENVIRONMENT=y
 CONFIG_SYS_RELOC_GD_ENV_ADDR=y
+CONFIG_SPL_ENV_IS_NOWHERE=y
 CONFIG_NET_RANDOM_ETHADDR=y
 CONFIG_IP_DEFRAG=y
 CONFIG_TFTP_BLOCKSIZE=1536
@@ -79,8 +86,6 @@
 CONFIG_DFU_MMC=y
 CONFIG_DFU_MTD=y
 CONFIG_DFU_RAM=y
-CONFIG_DFU_VIRT=y
-CONFIG_SET_DFU_ALT_INFO=y
 CONFIG_GPIO_HOG=y
 CONFIG_DM_HWSPINLOCK=y
 CONFIG_HWSPINLOCK_STM32=y
@@ -106,18 +111,20 @@
 CONFIG_DWC_ETH_QOS=y
 CONFIG_KS8851_MLL=y
 CONFIG_PHY=y
+CONFIG_SPL_PHY=y
 CONFIG_PHY_STM32_USBPHYC=y
 CONFIG_PINCONF=y
 # CONFIG_SPL_PINCTRL_FULL is not set
 CONFIG_PINCTRL_STMFX=y
 CONFIG_DM_PMIC=y
-# CONFIG_SPL_PMIC_CHILDREN is not set
 CONFIG_PMIC_STPMIC1=y
 CONFIG_DM_REGULATOR=y
+CONFIG_SPL_DM_REGULATOR=y
 CONFIG_DM_REGULATOR_FIXED=y
 CONFIG_DM_REGULATOR_GPIO=y
 CONFIG_DM_REGULATOR_STM32_VREFBUF=y
 CONFIG_DM_REGULATOR_STPMIC1=y
+CONFIG_SPL_DM_REGULATOR_STPMIC1=y
 CONFIG_REMOTEPROC_STM32_COPRO=y
 CONFIG_DM_RTC=y
 CONFIG_RTC_STM32=y
@@ -129,8 +136,10 @@
 CONFIG_SYSRESET_SYSCON=y
 CONFIG_USB=y
 CONFIG_DM_USB_GADGET=y
+CONFIG_SPL_DM_USB_GADGET=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_GENERIC=y
+CONFIG_USB_DWC2=y
 CONFIG_USB_HOST_ETHER=y
 CONFIG_USB_ETHER_ASIX=y
 CONFIG_USB_GADGET=y
diff --git a/drivers/mtd/nand/raw/stm32_fmc2_nand.c b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
index eee6594..fb3279b 100644
--- a/drivers/mtd/nand/raw/stm32_fmc2_nand.c
+++ b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
@@ -12,6 +12,7 @@
 #include <log.h>
 #include <nand.h>
 #include <reset.h>
+#include <asm/gpio.h>
 #include <dm/device_compat.h>
 #include <linux/bitfield.h>
 #include <linux/bitops.h>
@@ -149,6 +150,7 @@
 struct stm32_fmc2_nand {
 	struct nand_chip chip;
 	struct stm32_fmc2_timings timings;
+	struct gpio_desc wp_gpio;
 	int ncs;
 	int cs_used[FMC2_MAX_CE];
 };
@@ -824,6 +826,9 @@
 		nand->cs_used[i] = cs[i];
 	}
 
+	gpio_request_by_name_nodev(node, "wp-gpios", 0, &nand->wp_gpio,
+				   GPIOD_IS_OUT | GPIOD_IS_OUT_ACTIVE);
+
 	nand->chip.flash_node = node;
 
 	return 0;
@@ -972,6 +977,10 @@
 	chip->ecc.size = FMC2_ECC_STEP_SIZE;
 	chip->ecc.strength = FMC2_ECC_BCH8;
 
+	/* Disable Write Protect */
+	if (dm_gpio_is_valid(&nand->wp_gpio))
+		dm_gpio_set_value(&nand->wp_gpio, 0);
+
 	ret = nand_scan_ident(mtd, nand->ncs, NULL);
 	if (ret)
 		return ret;
diff --git a/drivers/ram/stm32mp1/stm32mp1_ddr.c b/drivers/ram/stm32mp1/stm32mp1_ddr.c
index 4d78aa5..528a171 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ddr.c
+++ b/drivers/ram/stm32mp1/stm32mp1_ddr.c
@@ -27,6 +27,8 @@
 #define RCC_DDRITFCR_DPHYAPBRST		(BIT(17))
 #define RCC_DDRITFCR_DPHYRST		(BIT(18))
 #define RCC_DDRITFCR_DPHYCTLRST		(BIT(19))
+#define RCC_DDRITFCR_DDRCKMOD_MASK	GENMASK(22, 20)
+#define RCC_DDRITFCR_DDRCKMOD_ASR	BIT(20)
 
 struct reg_desc {
 	const char *name;
@@ -651,6 +653,26 @@
 	wait_sw_done_ack(ctl);
 }
 
+static void stm32mp1_asr_enable(struct ddr_info *priv)
+{
+	struct stm32mp1_ddrctl *ctl = priv->ctl;
+
+	clrsetbits_le32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCKMOD_MASK,
+			RCC_DDRITFCR_DDRCKMOD_ASR);
+
+	start_sw_done(ctl);
+
+	setbits_le32(&ctl->hwlpctl, DDRCTRL_HWLPCTL_HW_LP_EN);
+	writel(DDRCTRL_PWRTMG_POWERDOWN_TO_X32(0x10) |
+	       DDRCTRL_PWRTMG_SELFREF_TO_X32(0x01),
+	       &ctl->pwrtmg);
+	setbits_le32(&ctl->pwrctl, DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+	setbits_le32(&ctl->pwrctl, DDRCTRL_PWRCTL_SELFREF_EN);
+
+	setbits_le32(&ctl->dfimisc, DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	wait_sw_done_ack(ctl);
+}
+
 /* board-specific DDR power initializations. */
 __weak int board_ddr_power_init(enum ddr_type ddr_type)
 {
@@ -822,6 +844,9 @@
 	stm32mp1_refresh_restore(priv->ctl, config->c_reg.rfshctl3,
 				 config->c_reg.pwrctl);
 
+/* Enable auto-self-refresh, which saves a bit of power at runtime. */
+	stm32mp1_asr_enable(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);
diff --git a/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h b/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
index f1a26e3..42be1ba 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
+++ b/drivers/ram/stm32mp1/stm32mp1_ddr_regs.h
@@ -265,8 +265,14 @@
 
 #define DDRCTRL_PWRCTL_SELFREF_EN		BIT(0)
 #define DDRCTRL_PWRCTL_POWERDOWN_EN		BIT(1)
+#define DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE	BIT(3)
 #define DDRCTRL_PWRCTL_SELFREF_SW		BIT(5)
 
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32(n)	(((n) & 0xff) << 16)
+#define DDRCTRL_PWRTMG_POWERDOWN_TO_X32(n)	((n) & 0x1f)
+
+#define DDRCTRL_HWLPCTL_HW_LP_EN		BIT(0)
+
 #define DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH	BIT(0)
 
 #define DDRCTRL_RFSHTMG_T_RFC_NOM_X1_X32_MASK	GENMASK(27, 16)
diff --git a/drivers/usb/gadget/dwc2_udc_otg.c b/drivers/usb/gadget/dwc2_udc_otg.c
index 2748270..77988f7 100644
--- a/drivers/usb/gadget/dwc2_udc_otg.c
+++ b/drivers/usb/gadget/dwc2_udc_otg.c
@@ -996,8 +996,9 @@
 	plat->rx_fifo_sz = dev_read_u32_default(dev, "g-rx-fifo-size", 0);
 	plat->np_tx_fifo_sz = dev_read_u32_default(dev, "g-np-tx-fifo-size", 0);
 
-	plat->tx_fifo_sz_nb =
-		dev_read_size(dev, "g-tx-fifo-size") / sizeof(u32);
+	ret = dev_read_size(dev, "g-tx-fifo-size");
+	if (ret > 0)
+		plat->tx_fifo_sz_nb = ret / sizeof(u32);
 	if (plat->tx_fifo_sz_nb > DWC2_MAX_HW_ENDPOINTS)
 		plat->tx_fifo_sz_nb = DWC2_MAX_HW_ENDPOINTS;
 	if (plat->tx_fifo_sz_nb) {
diff --git a/drivers/video/stm32/stm32_ltdc.c b/drivers/video/stm32/stm32_ltdc.c
index 87e5fd5..e741e74 100644
--- a/drivers/video/stm32/stm32_ltdc.c
+++ b/drivers/video/stm32/stm32_ltdc.c
@@ -338,6 +338,7 @@
 	struct display_timing timings;
 	struct clk pclk;
 	struct reset_ctl rst;
+	ulong rate;
 	int ret;
 
 	priv->regs = (void *)dev_read_addr(dev);
@@ -375,13 +376,13 @@
 		}
 	}
 
-	ret = clk_set_rate(&pclk, timings.pixelclock.typ);
-	if (ret)
-		dev_warn(dev, "fail to set pixel clock %d hz\n",
-			 timings.pixelclock.typ);
+	rate = clk_set_rate(&pclk, timings.pixelclock.typ);
+	if (IS_ERR_VALUE(rate))
+		dev_warn(dev, "fail to set pixel clock %d hz, ret=%ld\n",
+			 timings.pixelclock.typ, rate);
 
 	dev_dbg(dev, "Set pixel clock req %d hz get %ld hz\n",
-		timings.pixelclock.typ, clk_get_rate(&pclk));
+		timings.pixelclock.typ, rate);
 
 	ret = reset_get_by_index(dev, 0, &rst);
 	if (ret) {
diff --git a/drivers/video/video-uclass.c b/drivers/video/video-uclass.c
index 7d499bc..88797d4 100644
--- a/drivers/video/video-uclass.c
+++ b/drivers/video/video-uclass.c
@@ -33,7 +33,8 @@
  * information represents the requires size and alignment of the frame buffer
  * for the device. The values can be an over-estimate but cannot be too
  * small. The actual values will be suppled (in the same manner) by the bind()
- * method after relocation.
+ * method after relocation. Additionally driver can allocate frame buffer
+ * itself by setting plat->base.
  *
  * This information is then picked up by video_reserve() which works out how
  * much memory is needed for all devices. This is allocated between
@@ -78,6 +79,10 @@
 	if (!plat->size)
 		return 0;
 
+	/* Allow drivers to allocate the frame buffer themselves */
+	if (plat->base)
+		return 0;
+
 	align = plat->align ? plat->align : 1 << 20;
 	base = *addrp - plat->size;
 	base &= ~(align - 1);
diff --git a/drivers/video/video_bmp.c b/drivers/video/video_bmp.c
index c8c3fd3..4d2d961 100644
--- a/drivers/video/video_bmp.c
+++ b/drivers/video/video_bmp.c
@@ -31,6 +31,18 @@
 }
 
 /**
+ * get_bmp_col_x2r10g10b10() - Convert a colour-table entry into a x2r10g10b10  pixel value
+ *
+ * Return: value to write to the x2r10g10b10 frame buffer for this palette entry
+ */
+static u32 get_bmp_col_x2r10g10b10(struct bmp_color_table_entry *cte)
+{
+	return ((cte->red << 22U) |
+		(cte->green << 12U) |
+		(cte->blue << 2U));
+}
+
+/**
  * write_pix8() - Write a pixel from a BMP image into the framebuffer
  *
  * This handles frame buffers with 8, 16, 24 or 32 bits per pixel
@@ -42,8 +54,8 @@
  *	which is either written directly (bpix == 8) or used to look up the
  *	palette to get a colour to write
  */
-static void write_pix8(u8 *fb, uint bpix, struct bmp_color_table_entry *palette,
-		       u8 *bmap)
+static void write_pix8(u8 *fb, uint bpix, enum video_format eformat,
+		       struct bmp_color_table_entry *palette, u8 *bmap)
 {
 	if (bpix == 8) {
 		*fb++ = *bmap;
@@ -57,6 +69,8 @@
 			*fb++ = cte->red;
 			*fb++ = cte->green;
 			*fb++ = cte->blue;
+		} else if (eformat == VIDEO_X2R10G10B10) {
+			*(u32 *)fb = get_bmp_col_x2r10g10b10(cte);
 		} else {
 			*fb++ = cte->blue;
 			*fb++ = cte->green;
@@ -66,28 +80,29 @@
 	}
 }
 
-static void draw_unencoded_bitmap(u8 **fbp, uint bpix, uchar *bmap,
+static void draw_unencoded_bitmap(u8 **fbp, uint bpix,
+				  enum video_format eformat, uchar *bmap,
 				  struct bmp_color_table_entry *palette,
 				  int cnt)
 {
 	u8 *fb = *fbp;
 
 	while (cnt > 0) {
-		write_pix8(fb, bpix, palette, bmap++);
+		write_pix8(fb, bpix, eformat, palette, bmap++);
 		fb += bpix / 8;
 		cnt--;
 	}
 	*fbp = fb;
 }
 
-static void draw_encoded_bitmap(u8 **fbp, uint bpix,
+static void draw_encoded_bitmap(u8 **fbp, uint bpix, enum video_format eformat,
 				struct bmp_color_table_entry *palette, u8 *bmap,
 				int cnt)
 {
 	u8 *fb = *fbp;
 
 	while (cnt > 0) {
-		write_pix8(fb, bpix, palette, bmap);
+		write_pix8(fb, bpix, eformat, palette, bmap);
 		fb += bpix / 8;
 		cnt--;
 	}
@@ -106,6 +121,7 @@
 	int x, y;
 	int decode = 1;
 	uint bytes_per_pixel = bpix / 8;
+	enum video_format eformat = priv->format;
 
 	debug("%s\n", __func__);
 	bmap = (uchar *)bmp + get_unaligned_le32(&bmp->header.data_offset);
@@ -148,7 +164,7 @@
 						else
 							cnt = runlen;
 						draw_unencoded_bitmap(
-							&fb, bpix,
+							&fb, bpix, eformat,
 							bmap, palette, cnt);
 					}
 					x += runlen;
@@ -173,8 +189,9 @@
 						cnt = width - x;
 					else
 						cnt = runlen;
-					draw_encoded_bitmap(&fb, bpix, palette,
-							    &bmap[1], cnt);
+					draw_encoded_bitmap(&fb, bpix, eformat,
+							    palette, &bmap[1],
+							    cnt);
 				}
 				x += runlen;
 			}
@@ -224,6 +241,7 @@
 	unsigned long width, height, byte_width;
 	unsigned long pwidth = priv->xsize;
 	unsigned colours, bpix, bmp_bpix;
+	enum video_format eformat;
 	struct bmp_color_table_entry *palette;
 	int hdr_size;
 	int ret;
@@ -245,6 +263,7 @@
 	colours = 1 << bmp_bpix;
 
 	bpix = VNBITS(priv->bpix);
+	eformat = priv->format;
 
 	if (bpix != 1 && bpix != 8 && bpix != 16 && bpix != 32) {
 		printf("Error: %d bit/pixel mode, but BMP has %d bit/pixel\n",
@@ -312,7 +331,7 @@
 		for (i = 0; i < height; ++i) {
 			WATCHDOG_RESET();
 			for (j = 0; j < width; j++) {
-				write_pix8(fb, bpix, palette, bmap);
+				write_pix8(fb, bpix, eformat, palette, bmap);
 				bmap++;
 				fb += bpix / 8;
 			}
@@ -345,6 +364,16 @@
 							(bmap[0] >> 3);
 						bmap += 3;
 						fb += 2;
+					} else if (eformat == VIDEO_X2R10G10B10) {
+						u32 pix;
+
+						pix = *bmap++ << 2U;
+						pix |= *bmap++ << 12U;
+						pix |= *bmap++ << 22U;
+						*fb++ = pix & 0xff;
+						*fb++ = (pix >> 8) & 0xff;
+						*fb++ = (pix >> 16) & 0xff;
+						*fb++ = pix >> 24;
 					} else {
 						*fb++ = *bmap++;
 						*fb++ = *bmap++;
@@ -361,10 +390,23 @@
 		if (IS_ENABLED(CONFIG_BMP_32BPP)) {
 			for (i = 0; i < height; ++i) {
 				for (j = 0; j < width; j++) {
-					*fb++ = *bmap++;
-					*fb++ = *bmap++;
-					*fb++ = *bmap++;
-					*fb++ = *bmap++;
+					if (eformat == VIDEO_X2R10G10B10) {
+						u32 pix;
+
+						pix = *bmap++ << 2U;
+						pix |= *bmap++ << 12U;
+						pix |= *bmap++ << 22U;
+						pix |= (*bmap++ >> 6) << 30U;
+						*fb++ = pix & 0xff;
+						*fb++ = (pix >> 8) & 0xff;
+						*fb++ = (pix >> 16) & 0xff;
+						*fb++ = pix >> 24;
+					} else {
+						*fb++ = *bmap++;
+						*fb++ = *bmap++;
+						*fb++ = *bmap++;
+						*fb++ = *bmap++;
+					}
 				}
 				fb -= priv->line_length + width * (bpix / 8);
 			}
diff --git a/include/configs/nokia_rx51.h b/include/configs/nokia_rx51.h
index 9be64c3..e837b12 100644
--- a/include/configs/nokia_rx51.h
+++ b/include/configs/nokia_rx51.h
@@ -70,19 +70,12 @@
 
 #define CONFIG_SYS_ONENAND_BASE		ONENAND_MAP
 
-/*
- * Framebuffer
- */
-/* Video console */
-#define VIDEO_FB_16BPP_PIXEL_SWAP
-#define VIDEO_FB_16BPP_WORD_SWAP
-
 /* Environment information */
 #define CONFIG_EXTRA_ENV_SETTINGS \
 	"usbtty=cdc_acm\0" \
 	"stdin=usbtty,serial,keyboard\0" \
-	"stdout=usbtty,serial,vga\0" \
-	"stderr=usbtty,serial,vga\0" \
+	"stdout=usbtty,serial,vidconsole\0" \
+	"stderr=usbtty,serial,vidconsole\0" \
 	"slide=gpio input " __stringify(GPIO_SLIDE) "\0" \
 	"switchmmc=mmc dev ${mmcnum}\0" \
 	"kernaddr=0x82008000\0" \