14 files changed, 558 insertions, 330 deletions

diff --git a/arch/arm/mach-stm32mp/Kconfig b/arch/arm/mach-stm32mp/Kconfig
index 9b55a3d218..524d282a1d 100644
--- a/arch/arm/mach-stm32mp/Kconfig
+++ b/arch/arm/mach-stm32mp/Kconfig
@@ -1,15 +1,58 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
 if ARCH_STM32MP
 
 config ARCH_NR_GPIO
 	int
 	default 416
 
+config ARCH_STM32MP13
+	select ARM_PSCI_CLIENT
+	bool
+
 config ARCH_STM32MP157
 	select ARM_PSCI_CLIENT
 	bool
 
-config MACH_STM32MP157C_DK2
+config MACH_STM32MP13XX_DK
+	select ARCH_STM32MP13
+	bool "STM32MP135F DK board"
+
+config MACH_STM32MP15XX_DKX
+	select ARCH_STM32MP157
+	bool "STM32MP157 DK1 and DK2 boards"
+	help
+	  builds a single barebox-stm32mp15xx-dkx.stm32 that can be deployed
+	  as SSBL on both the stm32mp157a-dk1 and stm32mp157c-dk2
+
+config MACH_LXA_MC1
+	select ARCH_STM32MP157
+	bool "Linux Automation MC-1 board"
+
+config MACH_SEEED_ODYSSEY
+	select ARCH_STM32MP157
+	bool "Seeed Studio Odyssey"
+
+config MACH_STM32MP15X_EV1
+	select ARCH_STM32MP157
+	bool "STM32MP15X-EV1 board"
+	help
+	  builds a single barebox-stm32mp15x-ev1.stm32 that can be deployed
+	  as SSBL on any STM32MP15X-EVAL platform, like the
+	  STM32MP157C-EV1
+
+config MACH_PROTONIC_STM32MP1
+	select ARCH_STM32MP157
+	bool "Protonic PRTT1L family of boards"
+	help
+	  Builds all barebox-prtt1*.stm32 that can be deployed as SSBL
+	  on the respective PRTT1L family board
+
+config MACH_PHYTEC_PHYCORE_STM32MP1
 	select ARCH_STM32MP157
-	bool "STM32MP157C-DK2 board"
+	bool "phyCORE-STM32MP1"
+	help
+	  builds an additional barebox-phytec-phycore.stm32
+	  that can be deployed as SSBL on the phyCORE-STM32MP1
 
 endif
diff --git a/arch/arm/mach-stm32mp/Makefile b/arch/arm/mach-stm32mp/Makefile
index 6f49528892..837449150c 100644
--- a/arch/arm/mach-stm32mp/Makefile
+++ b/arch/arm/mach-stm32mp/Makefile
@@ -1,2 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
 obj-y := init.o
+obj-pbl-y := ddrctrl.o
+pbl-y := bl33-generic.o
 obj-$(CONFIG_BOOTM) += stm32image.o
+obj-$(CONFIG_BAREBOX_UPDATE) += bbu.o
diff --git a/arch/arm/mach-stm32mp/bbu.c b/arch/arm/mach-stm32mp/bbu.c
new file mode 100644
index 0000000000..5d6d61db7d
--- /dev/null
+++ b/arch/arm/mach-stm32mp/bbu.c
@@ -0,0 +1,197 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#define pr_fmt(fmt) "stm32mp-bbu: " fmt
+#include <common.h>
+#include <malloc.h>
+#include <bbu.h>
+#include <filetype.h>
+#include <errno.h>
+#include <fs.h>
+#include <fcntl.h>
+#include <linux/sizes.h>
+#include <linux/stat.h>
+#include <ioctl.h>
+#include <mach/stm32mp/bbu.h>
+#include <libfile.h>
+#include <linux/bitfield.h>
+
+#define STM32MP_BBU_IMAGE_HAVE_FSBL		BIT(0)
+#define STM32MP_BBU_IMAGE_HAVE_FIP		BIT(1)
+
+struct stm32mp_bbu_handler {
+	struct bbu_handler handler;
+	loff_t offset;
+};
+
+#define to_stm32mp_bbu_handler(h) container_of(h, struct stm32mp_bbu_handler, h)
+
+static int stm32mp_bbu_gpt_part_update(struct bbu_handler *handler,
+				       const struct bbu_data *data,
+				       const char *part, bool optional)
+{
+	struct bbu_data gpt_data = *data;
+	struct stat st;
+	int ret;
+
+	gpt_data.devicefile = basprintf("%s.%s", gpt_data.devicefile, part);
+	if (!gpt_data.devicefile)
+		return -ENOMEM;
+
+	pr_debug("Attempting %s update\n", gpt_data.devicefile);
+
+	ret = stat(gpt_data.devicefile, &st);
+	if (ret == -ENOENT) {
+		if (optional)
+			return 0;
+		pr_err("Partition %s does not exist\n", gpt_data.devicefile);
+	}
+	if (ret)
+		goto out;
+
+	ret = bbu_std_file_handler(handler, &gpt_data);
+out:
+	kfree_const(gpt_data.devicefile);
+	return ret;
+}
+
+static int stm32mp_bbu_mmc_update(struct bbu_handler *handler,
+				  struct bbu_data *data)
+{
+	struct stm32mp_bbu_handler *priv = to_stm32mp_bbu_handler(handler);
+	int fd, ret;
+	size_t image_len = data->len;
+	const void *buf = data->image;
+	struct stat st;
+
+	pr_debug("Attempting eMMC boot partition update\n");
+
+	ret = bbu_confirm(data);
+	if (ret)
+		return ret;
+
+	fd = open(data->devicefile, O_RDWR);
+	if (fd < 0)
+		return fd;
+
+	ret = fstat(fd, &st);
+	if (ret)
+		goto close;
+
+	if (st.st_size < priv->offset || image_len > st.st_size - priv->offset) {
+		ret = -ENOSPC;
+		goto close;
+	}
+
+	ret = pwrite_full(fd, buf, image_len, priv->offset);
+	if (ret < 0)
+		pr_err("writing to %s failed with %pe\n", data->devicefile, ERR_PTR(ret));
+
+close:
+	close(fd);
+
+	return ret < 0 ? ret : 0;
+}
+
+/*
+ * TF-A compiled with STM32_EMMC_BOOT will first check for FIP image
+ * at offset SZ_256K and then in GPT partition of that name.
+ */
+static int stm32mp_bbu_mmc_fip_handler(struct bbu_handler *handler,
+				       struct bbu_data *data)
+{
+	struct stm32mp_bbu_handler *priv = to_stm32mp_bbu_handler(handler);
+	enum filetype filetype;
+	int image_flags = 0, ret;
+	bool is_emmc = true;
+
+	filetype = file_detect_type(data->image, data->len);
+
+	switch (filetype) {
+	case filetype_stm32_image_fsbl_v1:
+		priv->offset = 0;
+		image_flags |= STM32MP_BBU_IMAGE_HAVE_FSBL;
+		if (data->len > SZ_256K)
+			image_flags |= STM32MP_BBU_IMAGE_HAVE_FIP;
+		break;
+	default:
+		if (!bbu_force(data, "incorrect image type. Expected: %s, got %s",
+				file_type_to_string(filetype_fip),
+				file_type_to_string(filetype)))
+			return -EINVAL;
+		/* If forced assume it's a SSBL */
+		filetype = filetype_fip;
+		fallthrough;
+	case filetype_fip:
+		priv->offset = SZ_256K;
+		image_flags |= STM32MP_BBU_IMAGE_HAVE_FIP;
+		break;
+	}
+
+	pr_debug("Handling %s\n", file_type_to_string(filetype));
+
+	handler->flags |= BBU_HANDLER_FLAG_MMC_BOOT_ACK;
+
+	ret = bbu_mmcboot_handler(handler, data, stm32mp_bbu_mmc_update);
+	if (ret == -ENOENT) {
+		pr_debug("Not an eMMC, falling back to GPT fsbl1 partition\n");
+		is_emmc = false;
+		ret = 0;
+	}
+	if (ret < 0) {
+		pr_debug("eMMC boot update failed: %pe\n", ERR_PTR(ret));
+		return ret;
+	}
+
+	if (!is_emmc && (image_flags & STM32MP_BBU_IMAGE_HAVE_FSBL)) {
+		struct bbu_data fsbl1_data = *data;
+
+		fsbl1_data.len = min_t(size_t, fsbl1_data.len, SZ_256K);
+
+		/*
+		 * BootROM tells TF-A which fsbl slot was booted in r0, but TF-A
+		 * doesn't yet propagate this to us, so for now always flash
+		 * fsbl1
+		 */
+		ret = stm32mp_bbu_gpt_part_update(handler, &fsbl1_data, "fsbl1", false);
+	}
+
+	if (ret == 0 && (image_flags & STM32MP_BBU_IMAGE_HAVE_FIP)) {
+		struct bbu_data fip_data = *data;
+
+		if (image_flags & STM32MP_BBU_IMAGE_HAVE_FSBL) {
+			fip_data.image += SZ_256K;
+			fip_data.len -= SZ_256K;
+		}
+
+		/* No fip GPT partition in eMMC user area is usually ok, as
+		 * that means TF-A is configured to load FIP from eMMC boot part
+		 */
+		ret = stm32mp_bbu_gpt_part_update(handler, &fip_data, "fip", is_emmc);
+	}
+
+	if (ret < 0)
+		pr_debug("eMMC user area update failed: %pe\n", ERR_PTR(ret));
+
+	return ret;
+}
+
+int stm32mp_bbu_mmc_fip_register(const char *name,
+				 const char *devicefile,
+				 unsigned long flags)
+{
+	struct stm32mp_bbu_handler *priv;
+	int ret;
+
+	priv = xzalloc(sizeof(*priv));
+
+	priv->handler.flags = flags;
+	priv->handler.devicefile = devicefile;
+	priv->handler.name = name;
+	priv->handler.handler = stm32mp_bbu_mmc_fip_handler;
+
+	ret = bbu_register_handler(&priv->handler);
+	if (ret)
+		free(priv);
+
+	return ret;
+}
diff --git a/arch/arm/mach-stm32mp/bl33-generic.c b/arch/arm/mach-stm32mp/bl33-generic.c
new file mode 100644
index 0000000000..dda0135a07
--- /dev/null
+++ b/arch/arm/mach-stm32mp/bl33-generic.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <mach/stm32mp/entry.h>
+#include <debug_ll.h>
+
+/*
+ * barebox-dt-2nd.img expects being loaded at an offset, so the
+ * stack can grow down from entry point. The STM32MP TF-A default
+ * is to not have an offset. This stm32mp specific entry points
+ * avoids this issue by setting up a 64 byte stack after end of
+ * barebox and by asking the memory controller about RAM size
+ * instead of parsing it out of the DT.
+ *
+ * When using OP-TEE, ensure CONFIG_OPTEE_SIZE is appopriately set.
+ */
+
+ENTRY_FUNCTION(start_stm32mp_bl33, r0, r1, r2)
+{
+	stm32mp_cpu_lowlevel_init();
+
+	putc_ll('>');
+
+	stm32mp1_barebox_entry((void *)r2);
+}
diff --git a/arch/arm/mach-stm32mp/ddrctrl.c b/arch/arm/mach-stm32mp/ddrctrl.c
new file mode 100644
index 0000000000..f198ee196c
--- /dev/null
+++ b/arch/arm/mach-stm32mp/ddrctrl.c
@@ -0,0 +1,161 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 Ahmad Fatoum <a.fatoum@pengutronix.de>
+ */
+
+#include <common.h>
+#include <init.h>
+#include <mach/stm32mp/ddr_regs.h>
+#include <mach/stm32mp/entry.h>
+#include <mach/stm32mp/stm32.h>
+#include <mach/stm32mp/revision.h>
+#include <asm/barebox-arm.h>
+#include <asm/memory.h>
+#include <pbl.h>
+#include <io.h>
+
+#define ADDRMAP1_BANK_B0	GENMASK( 5,  0)
+#define ADDRMAP1_BANK_B1	GENMASK(13,  8)
+#define ADDRMAP1_BANK_B2	GENMASK(21, 16)
+
+#define ADDRMAP2_COL_B2		GENMASK( 3,  0)
+#define ADDRMAP2_COL_B3		GENMASK(11,  8)
+#define ADDRMAP2_COL_B4		GENMASK(19, 16)
+#define ADDRMAP2_COL_B5		GENMASK(27, 24)
+
+#define ADDRMAP3_COL_B6		GENMASK( 3,  0)
+#define ADDRMAP3_COL_B7		GENMASK(11,  8)
+#define ADDRMAP3_COL_B8		GENMASK(19, 16)
+#define ADDRMAP3_COL_B9		GENMASK(27, 24)
+
+#define ADDRMAP4_COL_B10	GENMASK( 3,  0)
+#define ADDRMAP4_COL_B11	GENMASK(11,  8)
+
+#define ADDRMAP5_ROW_B0		GENMASK( 3,  0)
+#define ADDRMAP5_ROW_B1		GENMASK(11,  8)
+#define ADDRMAP5_ROW_B2_10	GENMASK(19, 16)
+#define ADDRMAP5_ROW_B11	GENMASK(27, 24)
+
+#define ADDRMAP6_ROW_B12	GENMASK( 3,  0)
+#define ADDRMAP6_ROW_B13	GENMASK(11,  8)
+#define ADDRMAP6_ROW_B14	GENMASK(19, 16)
+#define ADDRMAP6_ROW_B15	GENMASK(27, 24)
+
+#define ADDRMAP9_ROW_B2		GENMASK( 3,  0)
+#define ADDRMAP9_ROW_B3		GENMASK(11,  8)
+#define ADDRMAP9_ROW_B4		GENMASK(19, 16)
+#define ADDRMAP9_ROW_B5		GENMASK(27, 24)
+
+#define ADDRMAP10_ROW_B6	GENMASK( 3,  0)
+#define ADDRMAP10_ROW_B7	GENMASK(11,  8)
+#define ADDRMAP10_ROW_B8	GENMASK(19, 16)
+#define ADDRMAP10_ROW_B9	GENMASK(27, 24)
+
+#define ADDRMAP11_ROW_B10	GENMASK( 3, 0)
+
+#define LINE_UNUSED(reg, mask) (((reg) & (mask)) == (mask))
+
+enum ddrctrl_buswidth {
+	BUSWIDTH_FULL = 0,
+	BUSWIDTH_HALF = 1,
+	BUSWIDTH_QUARTER = 2
+};
+
+static unsigned long ddrctrl_addrmap_ramsize(struct stm32mp1_ddrctl __iomem *d,
+					     enum ddrctrl_buswidth buswidth,
+					     unsigned nb_bytes)
+{
+	unsigned banks = 3, cols = 12, rows = 16;
+	u32 reg;
+
+	cols += buswidth;
+
+	reg = readl(&d->addrmap1);
+	if (LINE_UNUSED(reg, ADDRMAP1_BANK_B2)) banks--;
+	if (LINE_UNUSED(reg, ADDRMAP1_BANK_B1)) banks--;
+	if (LINE_UNUSED(reg, ADDRMAP1_BANK_B0)) banks--;
+
+	reg = readl(&d->addrmap2);
+	if (LINE_UNUSED(reg, ADDRMAP2_COL_B5)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP2_COL_B4)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP2_COL_B3)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP2_COL_B2)) cols--;
+
+	reg = readl(&d->addrmap3);
+	if (LINE_UNUSED(reg, ADDRMAP3_COL_B9)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP3_COL_B8)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP3_COL_B7)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP3_COL_B6)) cols--;
+
+	reg = readl(&d->addrmap4);
+	if (LINE_UNUSED(reg, ADDRMAP4_COL_B11)) cols--;
+	if (LINE_UNUSED(reg, ADDRMAP4_COL_B10)) cols--;
+
+	reg = readl(&d->addrmap5);
+	if (LINE_UNUSED(reg, ADDRMAP5_ROW_B11)) rows--;
+
+	reg = readl(&d->addrmap6);
+	if (LINE_UNUSED(reg, ADDRMAP6_ROW_B15)) rows--;
+	if (LINE_UNUSED(reg, ADDRMAP6_ROW_B14)) rows--;
+	if (LINE_UNUSED(reg, ADDRMAP6_ROW_B13)) rows--;
+	if (LINE_UNUSED(reg, ADDRMAP6_ROW_B12)) rows--;
+
+	return memory_sdram_size(cols, rows, BIT(banks),
+				 DIV_ROUND_UP(nb_bytes, BIT(buswidth)));
+}
+
+static inline unsigned ddrctrl_ramsize(void __iomem *base, unsigned nb_bytes)
+{
+	struct stm32mp1_ddrctl __iomem *ddrctl = base;
+	unsigned buswidth = readl(&ddrctl->mstr) & DDRCTRL_MSTR_DATA_BUS_WIDTH_MASK;
+	buswidth >>= DDRCTRL_MSTR_DATA_BUS_WIDTH_SHIFT;
+
+	return ddrctrl_addrmap_ramsize(ddrctl, buswidth, nb_bytes);
+}
+
+static inline unsigned stm32mp1_ddrctrl_ramsize(void)
+{
+	u32 nb_bytes = 4;
+
+	if (cpu_stm32_is_stm32mp13())
+		nb_bytes /= 2;
+
+	return ddrctrl_ramsize(IOMEM(STM32_DDRCTL_BASE), nb_bytes);
+}
+
+void __noreturn __prereloc stm32mp1_barebox_entry(void *boarddata)
+{
+	barebox_arm_entry(STM32_DDR_BASE, stm32mp1_ddrctrl_ramsize(), boarddata);
+}
+
+
+static int stm32mp1_ddr_probe(struct device *dev)
+{
+	struct resource *iores;
+	void __iomem *base;
+	unsigned long nb_bytes;
+
+	iores = dev_request_mem_resource(dev, 0);
+	if (IS_ERR(iores))
+		return PTR_ERR(iores);
+	base = IOMEM(iores->start);
+
+	nb_bytes = (unsigned long)device_get_match_data(dev);
+
+	return arm_add_mem_device("ram0", STM32_DDR_BASE,
+				  ddrctrl_ramsize(base, nb_bytes));
+}
+
+static __maybe_unused struct of_device_id stm32mp1_ddr_dt_ids[] = {
+	{ .compatible = "st,stm32mp1-ddr", .data = (void *)4 },
+	{ .compatible = "st,stm32mp13-ddr", .data = (void *)2 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, stm32mp1_ddr_dt_ids);
+
+static struct driver stm32mp1_ddr_driver = {
+	.name   = "stm32mp1-ddr",
+	.probe  = stm32mp1_ddr_probe,
+	.of_compatible = DRV_OF_COMPAT(stm32mp1_ddr_dt_ids),
+};
+mem_platform_driver(stm32mp1_ddr_driver);
diff --git a/arch/arm/mach-stm32mp/include/mach/bbu.h b/arch/arm/mach-stm32mp/include/mach/bbu.h
deleted file mode 100644
index 8b9504400e..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/bbu.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef MACH_STM32MP_BBU_H_
-#define MACH_STM32MP_BBU_H_
-
-#include <bbu.h>
-
-static inline int stm32mp_bbu_mmc_register_handler(const char *name,
-						   const char *devicefile,
-						   unsigned long flags)
-{
-	return bbu_register_std_file_update(name, flags, devicefile,
-					    filetype_stm32_image_v1);
-}
-
-#endif /* MACH_STM32MP_BBU_H_ */
diff --git a/arch/arm/mach-stm32mp/include/mach/bootsource.h b/arch/arm/mach-stm32mp/include/mach/bootsource.h
deleted file mode 100644
index 1b6f562ac3..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/bootsource.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#ifndef __MACH_STM32_BOOTSOURCE_H__
-#define __MACH_STM32_BOOTSOURCE_H__
-
-enum stm32mp_boot_device {
-	STM32MP_BOOT_FLASH_SD		= 0x10, /* .. 0x13 */
-	STM32MP_BOOT_FLASH_EMMC		= 0x20, /* .. 0x23 */
-	STM32MP_BOOT_FLASH_NAND		= 0x30,
-	STM32MP_BOOT_FLASH_NAND_FMC	= 0x31,
-	STM32MP_BOOT_FLASH_NOR		= 0x40,
-	STM32MP_BOOT_FLASH_NOR_QSPI	= 0x41,
-	STM32MP_BOOT_SERIAL_UART	= 0x50, /* .. 0x58 */
-	STM32MP_BOOT_SERIAL_USB		= 0x60,
-	STM32MP_BOOT_SERIAL_USB_OTG	= 0x62,
-};
-
-enum stm32mp_forced_boot_mode {
-	STM32MP_BOOT_NORMAL	= 0x00,
-	STM32MP_BOOT_FASTBOOT	= 0x01,
-	STM32MP_BOOT_RECOVERY	= 0x02,
-	STM32MP_BOOT_STM32PROG	= 0x03,
-	STM32MP_BOOT_UMS_MMC0	= 0x10,
-	STM32MP_BOOT_UMS_MMC1	= 0x11,
-	STM32MP_BOOT_UMS_MMC2	= 0x12,
-};
-
-enum stm32mp_forced_boot_mode st32mp_get_forced_boot_mode(void);
-
-#endif
diff --git a/arch/arm/mach-stm32mp/include/mach/bsec.h b/arch/arm/mach-stm32mp/include/mach/bsec.h
deleted file mode 100644
index 559faaa2ba..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/bsec.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef __MACH_STM32_BSEC_H__
-#define __MACH_STM32_BSEC_H__
-
-#include <mach/smc.h>
-
-/* Return status */
-enum bsec_smc {
-	BSEC_SMC_OK		= 0,
-	BSEC_SMC_ERROR		= -1,
-	BSEC_SMC_DISTURBED	= -2,
-	BSEC_SMC_INVALID_PARAM	= -3,
-	BSEC_SMC_PROG_FAIL	= -4,
-	BSEC_SMC_LOCK_FAIL	= -5,
-	BSEC_SMC_WRITE_FAIL	= -6,
-	BSEC_SMC_SHADOW_FAIL	= -7,
-	BSEC_SMC_TIMEOUT	= -8,
-};
-
-/* Service for BSEC */
-enum bsec_field {
-	BSEC_SMC_READ_SHADOW	= 1,
-	BSEC_SMC_PROG_OTP	= 2,
-	BSEC_SMC_WRITE_SHADOW	= 3,
-	BSEC_SMC_READ_OTP	= 4,
-	BSEC_SMC_READ_ALL	= 5,
-	BSEC_SMC_WRITE_ALL	= 6,
-};
-
-static inline enum bsec_smc bsec_read_field(enum bsec_field field, unsigned *val)
-{
-	return stm32mp_smc(STM32_SMC_BSEC, BSEC_SMC_READ_SHADOW,
-			   field, 0, val);
-}
-
-static inline enum bsec_smc bsec_write_field(enum bsec_field field, unsigned val)
-{
-	return stm32mp_smc(STM32_SMC_BSEC, BSEC_SMC_WRITE_SHADOW,
-			   field, val, NULL);
-}
-
-#endif
diff --git a/arch/arm/mach-stm32mp/include/mach/debug_ll.h b/arch/arm/mach-stm32mp/include/mach/debug_ll.h
deleted file mode 100644
index 99fedb91fe..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/debug_ll.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef __MACH_STM32MP1_DEBUG_LL_H
-#define __MACH_STM32MP1_DEBUG_LL_H
-
-#include <io.h>
-#include <mach/stm32.h>
-
-#define DEBUG_LL_UART_ADDR	STM32_UART4_BASE
-
-#define CR1_OFFSET	0x00
-#define CR3_OFFSET	0x08
-#define BRR_OFFSET	0x0c
-#define ISR_OFFSET	0x1c
-#define ICR_OFFSET	0x20
-#define RDR_OFFSET	0x24
-#define TDR_OFFSET	0x28
-
-#define USART_ISR_TXE	BIT(7)
-
-static inline void PUTC_LL(int c)
-{
-	void __iomem *base = IOMEM(DEBUG_LL_UART_ADDR);
-
-	writel(c, base + TDR_OFFSET);
-
-	while ((readl(base + ISR_OFFSET) & USART_ISR_TXE) == 0);
-}
-
-#endif /* __MACH_STM32MP1_DEBUG_LL_H */
diff --git a/arch/arm/mach-stm32mp/include/mach/revision.h b/arch/arm/mach-stm32mp/include/mach/revision.h
deleted file mode 100644
index 387201421d..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/revision.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
-/*
- * Copyright (C) 2015-2017, STMicroelectronics - All Rights Reserved
- */
-
-#ifndef __MACH_CPUTYPE_H__
-#define __MACH_CPUTYPE_H__
-
-/* ID = Device Version (bit31:16) + Device Part Number (RPN) (bit15:0)*/
-#define CPU_STM32MP157Cxx	0x05000000
-#define CPU_STM32MP157Axx	0x05000001
-#define CPU_STM32MP153Cxx	0x05000024
-#define CPU_STM32MP153Axx	0x05000025
-#define CPU_STM32MP151Cxx	0x0500002E
-#define CPU_STM32MP151Axx	0x0500002F
-
-/* silicon revisions */
-#define CPU_REV_A	0x1000
-#define CPU_REV_B	0x2000
-
-int stm32mp_silicon_revision(void);
-int stm32mp_cputype(void);
-int stm32mp_package(void);
-
-#define cpu_is_stm32mp157c() (stm32mp_cputype() == CPU_STM32MP157Cxx)
-#define cpu_is_stm32mp157a() (stm32mp_cputype() == CPU_STM32MP157Axx)
-#define cpu_is_stm32mp153c() (stm32mp_cputype() == CPU_STM32MP153Cxx)
-#define cpu_is_stm32mp153a() (stm32mp_cputype() == CPU_STM32MP153Axx)
-#define cpu_is_stm32mp151c() (stm32mp_cputype() == CPU_STM32MP151Cxx)
-#define cpu_is_stm32mp151a() (stm32mp_cputype() == CPU_STM32MP151Axx)
-
-#endif /* __MACH_CPUTYPE_H__ */
diff --git a/arch/arm/mach-stm32mp/include/mach/smc.h b/arch/arm/mach-stm32mp/include/mach/smc.h
deleted file mode 100644
index 6b8e62bd53..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/smc.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef __MACH_STM32_SMC_H__
-#define __MACH_STM32_SMC_H__
-
-#include <linux/arm-smccc.h>
-
-/* Secure Service access from Non-secure */
-#define STM32_SMC_RCC                   0x82001000
-#define STM32_SMC_PWR                   0x82001001
-#define STM32_SMC_RTC                   0x82001002
-#define STM32_SMC_BSEC                  0x82001003
-
-/* Register access service use for RCC/RTC/PWR */
-#define STM32_SMC_REG_WRITE             0x1
-#define STM32_SMC_REG_SET               0x2
-#define STM32_SMC_REG_CLEAR             0x3
-
-static inline int stm32mp_smc(u32 svc, u8 op, u32 data1, u32 data2, u32 *val)
-{
-        struct arm_smccc_res res;
-
-        arm_smccc_smc(svc, op, data1, data2, 0, 0, 0, 0, &res);
-        if (val)
-                *val = res.a1;
-
-        return (int)res.a0;
-}
-
-#endif
diff --git a/arch/arm/mach-stm32mp/include/mach/stm32.h b/arch/arm/mach-stm32mp/include/mach/stm32.h
deleted file mode 100644
index f9bdb788b9..0000000000
--- a/arch/arm/mach-stm32mp/include/mach/stm32.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#ifndef _MACH_STM32_H_
-#define _MACH_STM32_H_
-
-/*
- * Peripheral memory map
- */
-#define STM32_RCC_BASE			0x50000000
-#define STM32_PWR_BASE			0x50001000
-#define STM32_DBGMCU_BASE		0x50081000
-#define STM32_BSEC_BASE			0x5C005000
-#define STM32_TZC_BASE			0x5C006000
-#define STM32_ETZPC_BASE		0x5C007000
-#define STM32_TAMP_BASE			0x5C00A000
-
-#define STM32_USART1_BASE		0x5C000000
-#define STM32_USART2_BASE		0x4000E000
-#define STM32_USART3_BASE		0x4000F000
-#define STM32_UART4_BASE		0x40010000
-#define STM32_UART5_BASE		0x40011000
-#define STM32_USART6_BASE		0x44003000
-#define STM32_UART7_BASE		0x40018000
-#define STM32_UART8_BASE		0x40019000
-
-#define STM32_SYSRAM_BASE		0x2FFC0000
-#define STM32_SYSRAM_SIZE		SZ_256K
-
-#define STM32_DDR_BASE			0xC0000000
-#define STM32_DDR_SIZE			SZ_1G
-
-#endif /* _MACH_STM32_H_ */
diff --git a/arch/arm/mach-stm32mp/init.c b/arch/arm/mach-stm32mp/init.c
index 7bad989a60..2eb8b6beec 100644
--- a/arch/arm/mach-stm32mp/init.c
+++ b/arch/arm/mach-stm32mp/init.c
@@ -8,31 +8,12 @@
 
 #include <common.h>
 #include <init.h>
-#include <mach/stm32.h>
-#include <mach/bsec.h>
-#include <mach/revision.h>
-#include <mach/bootsource.h>
+#include <mach/stm32mp/stm32.h>
+#include <mach/stm32mp/bsec.h>
+#include <mach/stm32mp/revision.h>
+#include <mach/stm32mp/bootsource.h>
 #include <bootsource.h>
-
-/* DBGMCU register */
-#define DBGMCU_IDC		(STM32_DBGMCU_BASE + 0x00)
-#define DBGMCU_APB4FZ1		(STM32_DBGMCU_BASE + 0x2C)
-#define DBGMCU_APB4FZ1_IWDG2	BIT(2)
-#define DBGMCU_IDC_DEV_ID_MASK	GENMASK(11, 0)
-#define DBGMCU_IDC_DEV_ID_SHIFT	0
-#define DBGMCU_IDC_REV_ID_MASK	GENMASK(31, 16)
-#define DBGMCU_IDC_REV_ID_SHIFT	16
-
-#define RCC_DBGCFGR		(STM32_RCC_BASE + 0x080C)
-#define RCC_DBGCFGR_DBGCKEN	BIT(8)
-
-/* BSEC OTP index */
-#define BSEC_OTP_RPN	1
-#define BSEC_OTP_PKG	16
-
-/* Device Part Number (RPN) = OTP_DATA1 lower 8 bits */
-#define RPN_SHIFT	0
-#define RPN_MASK	GENMASK(7, 0)
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
 
 /* Package = bit 27:29 of OTP16
  * - 100: LBGA448  (FFI) => AA = LFBGA 18x18mm 448 balls p. 0.8mm
@@ -61,12 +42,9 @@
 
 /* TAMP registers */
 #define TAMP_BACKUP_REGISTER(x)         (STM32_TAMP_BASE + 0x100 + 4 * x)
-/* secure access */
-#define TAMP_BACKUP_MAGIC_NUMBER        TAMP_BACKUP_REGISTER(4)
-#define TAMP_BACKUP_BRANCH_ADDRESS      TAMP_BACKUP_REGISTER(5)
 /* non secure access */
-#define TAMP_BOOT_CONTEXT               TAMP_BACKUP_REGISTER(20)
-#define TAMP_BOOTCOUNT                  TAMP_BACKUP_REGISTER(21)
+#define STM32MP13_TAMP_BOOT_CONTEXT     TAMP_BACKUP_REGISTER(30)
+#define STM32MP15_TAMP_BOOT_CONTEXT     TAMP_BACKUP_REGISTER(20)
 
 #define TAMP_BOOT_MODE_MASK             GENMASK(15, 8)
 #define TAMP_BOOT_MODE_SHIFT            8
@@ -75,16 +53,12 @@
 #define TAMP_BOOT_FORCED_MASK           GENMASK(7, 0)
 #define TAMP_BOOT_DEBUG_ON              BIT(16)
 
+#define FIXUP_CPU_MASK(num, mhz) (((num) << 16) | (mhz))
+#define FIXUP_CPU_NUM(mask) ((mask) >> 16)
+#define FIXUP_CPU_HZ(mask) (((mask) & GENMASK(15, 0)) * 1000UL * 1000UL)
 
-static enum stm32mp_forced_boot_mode __stm32mp_forced_boot_mode;
-enum stm32mp_forced_boot_mode st32mp_get_forced_boot_mode(void)
-{
-	return __stm32mp_forced_boot_mode;
-}
-
-static void setup_boot_mode(void)
+static void setup_boot_mode(u32 boot_ctx)
 {
-	u32 boot_ctx = readl(TAMP_BOOT_CONTEXT);
 	u32 boot_mode =
 		(boot_ctx & TAMP_BOOT_MODE_MASK) >> TAMP_BOOT_MODE_SHIFT;
 	int instance = (boot_mode & TAMP_BOOT_INSTANCE_MASK) - 1;
@@ -117,143 +91,174 @@ static void setup_boot_mode(void)
 		break;
 	}
 
-	__stm32mp_forced_boot_mode = boot_ctx & TAMP_BOOT_FORCED_MASK;
+	pr_debug("[boot_ctx=0x%x] => mode=0x%x, instance=%d\n",
+		 boot_ctx, boot_mode, instance);
 
-	pr_debug("[boot_ctx=0x%x] => mode=0x%x, instance=%d forced=0x%x\n",
-		 boot_ctx, boot_mode, instance, __stm32mp_forced_boot_mode);
-
-	bootsource_set(src);
-	bootsource_set_instance(instance);
-
-	/* clear TAMP for next reboot */
-	clrsetbits_le32(TAMP_BOOT_CONTEXT, TAMP_BOOT_FORCED_MASK,
-			STM32MP_BOOT_NORMAL);
+	bootsource_set_raw(src, instance);
 }
 
-static int __stm32mp_cputype;
-int stm32mp_cputype(void)
+static int get_cpu_package(u32 *pkg)
 {
-	return __stm32mp_cputype;
-}
+	int ret = bsec_read_field(BSEC_OTP_PKG, pkg);
+	if (ret)
+		return ret;
 
-static int __stm32mp_silicon_revision;
-int stm32mp_silicon_revision(void)
-{
-	return __stm32mp_silicon_revision;
+	*pkg = (*pkg >> PKG_SHIFT) & PKG_MASK;
+	return 0;
 }
 
-static int __stm32mp_package;
-int stm32mp_package(void)
+static int stm32mp15_fixup_cpus(struct device_node *root, void *_ctx)
 {
-	return __stm32mp_package;
-}
+	unsigned long ctx = (unsigned long)_ctx;
+	struct device_node *cpus_node, *np, *tmp;
 
-static inline u32 read_idc(void)
-{
-	setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
-	return readl(IOMEM(DBGMCU_IDC));
-}
+	cpus_node = of_find_node_by_name_address(root, "cpus");
+	if (!cpus_node)
+		return 0;
 
-/* Get Device Part Number (RPN) from OTP */
-static u32 get_cpu_rpn(u32 *rpn)
-{
-	int ret = bsec_read_field(BSEC_OTP_RPN, rpn);
-	if (ret)
-		return ret;
+	for_each_child_of_node_safe(cpus_node, tmp, np) {
+		u32 cpu_index;
 
-	*rpn = (*rpn >> RPN_SHIFT) & RPN_MASK;
-	return 0;
-}
+		if (of_property_read_u32(np, "reg", &cpu_index))
+			continue;
 
-static u32 get_cpu_revision(void)
-{
-	return (read_idc() & DBGMCU_IDC_REV_ID_MASK) >> DBGMCU_IDC_REV_ID_SHIFT;
+		if (cpu_index >= FIXUP_CPU_NUM(ctx)) {
+			of_delete_node(np);
+			continue;
+		}
+
+		of_property_write_u32(np, "clock-frequency", FIXUP_CPU_HZ(ctx));
+	}
+
+	return 0;
 }
 
-static u32 get_cpu_type(u32 *type)
+static int fixup_pinctrl(struct device_node *root, const char *compat, u32 pkg)
 {
-	u32 id;
-	int ret = get_cpu_rpn(type);
-	if (ret)
-		return ret;
+	struct device_node *np = of_find_compatible_node(root, NULL, compat);
+	if (!np)
+		return -ENODEV;
 
-	id = (read_idc() & DBGMCU_IDC_DEV_ID_MASK) >> DBGMCU_IDC_DEV_ID_SHIFT;
-	*type |= id << 16;
-	return 0;
+	return of_property_write_u32(np, "st,package", pkg);
 }
 
-static int get_cpu_package(u32 *pkg)
+static int stm32mp15_fixup_pkg(struct device_node *root, void *_pkg)
 {
-	int ret = bsec_read_field(BSEC_OTP_PKG, pkg);
+	unsigned long pkg = (unsigned long)_pkg;
+	int ret;
+
+	ret = fixup_pinctrl(root, "st,stm32mp157-pinctrl", pkg);
 	if (ret)
 		return ret;
 
-	*pkg = (*pkg >> PKG_SHIFT) & PKG_MASK;
-	return 0;
+	return fixup_pinctrl(root, "st,stm32mp157-z-pinctrl", pkg);
 }
 
-static int setup_cpu_type(void)
+static int stm32mp15_setup_cpu_type(void)
 {
-	const char *cputypestr;
-	const char *cpupkgstr;
+	unsigned long cpufixupctx = 0, pkgfixupctx = 0;
+	int cputype, package;
 
-	get_cpu_type(&__stm32mp_cputype);
-	switch (__stm32mp_cputype) {
+	__stm32mp15_get_cpu_type(&cputype);
+	switch (cputype) {
+	case CPU_STM32MP157Fxx:
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
+		break;
+	case CPU_STM32MP157Dxx:
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
+		break;
 	case CPU_STM32MP157Cxx:
-		cputypestr = "157C";
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP157Axx:
-		cputypestr = "157A";
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
+		break;
+	case CPU_STM32MP153Fxx:
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
+		break;
+	case CPU_STM32MP153Dxx:
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
 		break;
 	case CPU_STM32MP153Cxx:
-		cputypestr = "153C";
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP153Axx:
-		cputypestr = "153A";
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP151Cxx:
-		cputypestr = "151C";
+		cpufixupctx = FIXUP_CPU_MASK(1, 650);
 		break;
 	case CPU_STM32MP151Axx:
-		cputypestr = "151A";
+		cpufixupctx = FIXUP_CPU_MASK(1, 650);
+		break;
+	case CPU_STM32MP151Fxx:
+		cpufixupctx = FIXUP_CPU_MASK(1, 800);
+		break;
+	case CPU_STM32MP151Dxx:
+		cpufixupctx = FIXUP_CPU_MASK(1, 800);
 		break;
 	default:
-		cputypestr = "????";
 		break;
 	}
 
-	get_cpu_package(&__stm32mp_package );
-	switch (__stm32mp_package) {
+	get_cpu_package(&package);
+	switch (package) {
 	case PKG_AA_LBGA448:
-		cpupkgstr = "AA";
+		pkgfixupctx = STM32MP_PKG_AA;
 		break;
 	case PKG_AB_LBGA354:
-		cpupkgstr = "AB";
+		pkgfixupctx = STM32MP_PKG_AB;
 		break;
 	case PKG_AC_TFBGA361:
-		cpupkgstr = "AC";
+		pkgfixupctx = STM32MP_PKG_AC;
 		break;
 	case PKG_AD_TFBGA257:
-		cpupkgstr = "AD";
+		pkgfixupctx = STM32MP_PKG_AD;
 		break;
 	default:
-		cpupkgstr = "??";
 		break;
 	}
 
-	__stm32mp_silicon_revision = get_cpu_revision();
+	pr_debug("cputype = 0x%x, package = 0x%x\n", cputype, package);
+
+	if (cpufixupctx)
+		of_register_fixup(stm32mp15_fixup_cpus, (void*)cpufixupctx);
+	if (pkgfixupctx)
+		of_register_fixup(stm32mp15_fixup_pkg, (void*)pkgfixupctx);
 
-	pr_debug("cputype = 0x%x, package = 0x%x, revision = 0x%x\n",
-		 __stm32mp_cputype, __stm32mp_package, __stm32mp_silicon_revision);
-	pr_info("detected STM32MP%s%s Rev.%c\n", cputypestr, cpupkgstr,
-		(__stm32mp_silicon_revision >> 12) + 'A' - 1);
 	return 0;
 }
 
+static int __st32mp_soc;
+
+int stm32mp_soc(void)
+{
+	return __st32mp_soc;
+}
+
 static int stm32mp_init(void)
 {
-	setup_cpu_type();
-	setup_boot_mode();
+	u32 boot_ctx;
+
+	if (of_machine_is_compatible("st,stm32mp135"))
+		__st32mp_soc = 32135;
+	else if (of_machine_is_compatible("st,stm32mp151"))
+		__st32mp_soc = 32151;
+	else if (of_machine_is_compatible("st,stm32mp153"))
+		__st32mp_soc = 32153;
+	else if (of_machine_is_compatible("st,stm32mp157"))
+		__st32mp_soc = 32157;
+	else
+		return 0;
+
+	if (__st32mp_soc == 32135) {
+		boot_ctx = readl(STM32MP13_TAMP_BOOT_CONTEXT);
+	} else {
+		stm32mp15_setup_cpu_type();
+		boot_ctx = readl(STM32MP15_TAMP_BOOT_CONTEXT);
+	}
+
+	setup_boot_mode(boot_ctx);
 
 	return 0;
 }
diff --git a/arch/arm/mach-stm32mp/stm32image.c b/arch/arm/mach-stm32mp/stm32image.c
index 84975c5c3b..37d7c73120 100644
--- a/arch/arm/mach-stm32mp/stm32image.c
+++ b/arch/arm/mach-stm32mp/stm32image.c
@@ -1,3 +1,5 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
 #define pr_fmt(fmt) "stm32image: " fmt
 
 #include <bootm.h>
@@ -5,6 +7,7 @@
 #include <init.h>
 #include <memory.h>
 #include <linux/sizes.h>
+#include <mach/stm32mp/stm32.h>
 
 #define BAREBOX_STAGE2_OFFSET	256
 
@@ -38,13 +41,14 @@ static int do_bootm_stm32image(struct image_data *data)
 static struct image_handler image_handler_stm32_image_v1_handler = {
 	.name = "STM32 image (v1)",
 	.bootm = do_bootm_stm32image,
-	.filetype = filetype_stm32_image_v1,
+	.filetype = filetype_stm32_image_ssbl_v1,
 };
 
 static int stm32mp_register_stm32image_image_handler(void)
 {
-	register_image_handler(&image_handler_stm32_image_v1_handler);
+	if (!stm32mp_soc())
+		return 0;
 
-	return 0;
+	return register_image_handler(&image_handler_stm32_image_v1_handler);
 }
 late_initcall(stm32mp_register_stm32image_image_handler);