diff --git a/arch/arm/mach-stm32mp/init.c b/arch/arm/mach-stm32mp/init.c
index d687e44..7f687fa 100644
--- a/arch/arm/mach-stm32mp/init.c
+++ b/arch/arm/mach-stm32mp/init.c
@@ -13,6 +13,7 @@
 #include <mach/revision.h>
 #include <mach/bootsource.h>
 #include <bootsource.h>
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
 
 /* DBGMCU register */
 #define DBGMCU_IDC		(STM32_DBGMCU_BASE + 0x00)
@@ -221,61 +222,83 @@
 	return 0;
 }
 
+static int fixup_pinctrl(struct device_node *root, const char *compat, u32 pkg)
+{
+	struct device_node *np = of_find_compatible_node(root, NULL, compat);
+	if (!np)
+		return -ENODEV;
+
+	return of_property_write_u32(np, "st,package", pkg);
+}
+
+static int stm32mp15_fixup_pkg(struct device_node *root, void *_pkg)
+{
+	unsigned long pkg = (unsigned long)_pkg;
+	int ret;
+
+	ret = fixup_pinctrl(root, "st,stm32mp157-pinctrl", pkg);
+	if (ret)
+		return ret;
+
+	return fixup_pinctrl(root, "st,stm32mp157-z-pinctrl", pkg);
+}
+
 static int setup_cpu_type(void)
 {
 	const char *cputypestr, *cpupkgstr, *cpurevstr;
-	unsigned long fixupctx = 0;
+	unsigned long cpufixupctx = 0, pkgfixupctx = 0;
 	u32 pkg;
+	int ret;
 
 	get_cpu_type(&__stm32mp_cputype);
 	switch (__stm32mp_cputype) {
 	case CPU_STM32MP157Fxx:
 		cputypestr = "157F";
-		fixupctx = FIXUP_CPU_MASK(2, 800);
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
 		break;
 	case CPU_STM32MP157Dxx:
 		cputypestr = "157D";
-		fixupctx = FIXUP_CPU_MASK(2, 800);
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
 		break;
 	case CPU_STM32MP157Cxx:
 		cputypestr = "157C";
-		fixupctx = FIXUP_CPU_MASK(2, 650);
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP157Axx:
 		cputypestr = "157A";
-		fixupctx = FIXUP_CPU_MASK(2, 650);
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP153Fxx:
 		cputypestr = "153F";
-		fixupctx = FIXUP_CPU_MASK(2, 800);
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
 		break;
 	case CPU_STM32MP153Dxx:
 		cputypestr = "153D";
-		fixupctx = FIXUP_CPU_MASK(2, 800);
+		cpufixupctx = FIXUP_CPU_MASK(2, 800);
 		break;
 	case CPU_STM32MP153Cxx:
 		cputypestr = "153C";
-		fixupctx = FIXUP_CPU_MASK(2, 650);
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP153Axx:
 		cputypestr = "153A";
-		fixupctx = FIXUP_CPU_MASK(2, 650);
+		cpufixupctx = FIXUP_CPU_MASK(2, 650);
 		break;
 	case CPU_STM32MP151Cxx:
 		cputypestr = "151C";
-		fixupctx = FIXUP_CPU_MASK(1, 650);
+		cpufixupctx = FIXUP_CPU_MASK(1, 650);
 		break;
 	case CPU_STM32MP151Axx:
 		cputypestr = "151A";
-		fixupctx = FIXUP_CPU_MASK(1, 650);
+		cpufixupctx = FIXUP_CPU_MASK(1, 650);
 		break;
 	case CPU_STM32MP151Fxx:
 		cputypestr = "151F";
-		fixupctx = FIXUP_CPU_MASK(1, 800);
+		cpufixupctx = FIXUP_CPU_MASK(1, 800);
 		break;
 	case CPU_STM32MP151Dxx:
 		cputypestr = "151D";
-		fixupctx = FIXUP_CPU_MASK(1, 800);
+		cpufixupctx = FIXUP_CPU_MASK(1, 800);
 		break;
 	default:
 		cputypestr = "????";
@@ -286,15 +309,19 @@
 	switch (__stm32mp_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 = "??";
@@ -320,8 +347,14 @@
 		 __stm32mp_cputype, pkg, __stm32mp_silicon_revision);
 	pr_info("detected STM32MP%s%s Rev.%s\n", cputypestr, cpupkgstr, cpurevstr);
 
-	if (fixupctx)
-		return of_register_fixup(stm32mp15_fixup_cpus, (void*)fixupctx);
+	if (cpufixupctx) {
+		ret = of_register_fixup(stm32mp15_fixup_cpus, (void*)cpufixupctx);
+		if (ret)
+			return ret;
+	}
+
+	if (pkgfixupctx)
+		return of_register_fixup(stm32mp15_fixup_pkg, (void*)pkgfixupctx);
 
 	return 0;
 }