smt32mp: add setup_mac_address for stm32mp25

Add a function setup_mac_address() to update the MAC address from the
default location in OTP for stm32mp2 platform.

The max number of OTP for MAC address is increased to 8 for STM32MP25,
defined with get_eth_nb() and checked in setup_mac_address.

The MAC address FF:FF:FF:FF:FF:FF, the broadcast ethaddr, is a invalid
value used for unused MAC address slot in OTP, for example for board
with STM32MP25x part number allows up to 5 ethernet ports but it is not
supported by the hardware, without switch; the associated variable
"enetaddr%d" is not created.

Reviewed-by: Patrice Chotard <patrice.chotard@foss.st.com>
Signed-off-by: Patrick Delaunay <patrick.delaunay@foss.st.com>

3 files changed, 72 insertions, 57 deletions

diff --git a/arch/arm/mach-stm32mp/soc.c b/arch/arm/mach-stm32mp/soc.c
index ff70ebe974..fa56b0d2e0 100644
--- a/arch/arm/mach-stm32mp/soc.c
+++ b/arch/arm/mach-stm32mp/soc.c
@@ -5,10 +5,14 @@
 
 #include <env.h>
 #include <misc.h>
+#include <net.h>
 #include <asm/arch/sys_proto.h>
 #include <dm/device.h>
 #include <dm/uclass.h>
 
+/* max: 8 OTP for 5 mac address on stm32mp2*/
+#define MAX_NB_OTP	8
+
 /* used when CONFIG_DISPLAY_CPUINFO is activated */
 int print_cpuinfo(void)
 {
@@ -46,3 +50,69 @@ int setup_serial_number(void)
 
 	return 0;
 }
+
+/*
+ * If there is no MAC address in the environment, then it will be initialized
+ * (silently) from the value in the OTP.
+ */
+__weak int setup_mac_address(void)
+{
+	int ret;
+	int i;
+	u32 otp[MAX_NB_OTP];
+	uchar enetaddr[ARP_HLEN];
+	struct udevice *dev;
+	int nb_eth, nb_otp, index;
+
+	if (!IS_ENABLED(CONFIG_NET))
+		return 0;
+
+	nb_eth = get_eth_nb();
+	if (!nb_eth)
+		return 0;
+
+	/* 6 bytes for each MAC addr and 4 bytes for each OTP */
+	nb_otp = DIV_ROUND_UP(ARP_HLEN * nb_eth, 4);
+	if (nb_otp > MAX_NB_OTP) {
+		log_err("invalid number of OTP = %d, max = %d\n", nb_otp, MAX_NB_OTP);
+		return -EINVAL;
+	}
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_DRIVER_GET(stm32mp_bsec),
+					  &dev);
+	if (ret)
+		return ret;
+
+	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_MAC), otp, 4 * nb_otp);
+	if (ret < 0)
+		return ret;
+
+	for (index = 0; index < nb_eth; index++) {
+		/* MAC already in environment */
+		if (eth_env_get_enetaddr_by_index("eth", index, enetaddr))
+			continue;
+
+		for (i = 0; i < ARP_HLEN; i++)
+			enetaddr[i] = ((uint8_t *)&otp)[i + ARP_HLEN * index];
+
+		/* skip FF:FF:FF:FF:FF:FF */
+		if (is_broadcast_ethaddr(enetaddr))
+			continue;
+
+		if (!is_valid_ethaddr(enetaddr)) {
+			log_err("invalid MAC address %d in OTP %pM\n",
+				index, enetaddr);
+			return -EINVAL;
+		}
+		log_debug("OTP MAC address %d = %pM\n", index, enetaddr);
+		ret = eth_env_set_enetaddr_by_index("eth", index, enetaddr);
+		if (ret) {
+			log_err("Failed to set mac address %pM from OTP: %d\n",
+				enetaddr, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/cpu.c b/arch/arm/mach-stm32mp/stm32mp1/cpu.c
index f84cb26fa5..524778f00c 100644
--- a/arch/arm/mach-stm32mp/stm32mp1/cpu.c
+++ b/arch/arm/mach-stm32mp/stm32mp1/cpu.c
@@ -14,8 +14,8 @@
 #include <log.h>
 #include <lmb.h>
 #include <misc.h>
-#include <net.h>
 #include <spl.h>
+#include <asm/cache.h>
 #include <asm/io.h>
 #include <asm/arch/stm32.h>
 #include <asm/arch/sys_proto.h>
@@ -280,62 +280,6 @@ static void setup_boot_mode(void)
 	clrsetbits_le32(TAMP_BOOT_CONTEXT, TAMP_BOOT_FORCED_MASK, BOOT_NORMAL);
 }
 
-/*
- * If there is no MAC address in the environment, then it will be initialized
- * (silently) from the value in the OTP.
- */
-__weak int setup_mac_address(void)
-{
-	int ret;
-	int i;
-	u32 otp[3];
-	uchar enetaddr[6];
-	struct udevice *dev;
-	int nb_eth, nb_otp, index;
-
-	if (!IS_ENABLED(CONFIG_NET))
-		return 0;
-
-	nb_eth = get_eth_nb();
-
-	/* 6 bytes for each MAC addr and 4 bytes for each OTP */
-	nb_otp = DIV_ROUND_UP(6 * nb_eth, 4);
-
-	ret = uclass_get_device_by_driver(UCLASS_MISC,
-					  DM_DRIVER_GET(stm32mp_bsec),
-					  &dev);
-	if (ret)
-		return ret;
-
-	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_MAC), otp, 4 * nb_otp);
-	if (ret < 0)
-		return ret;
-
-	for (index = 0; index < nb_eth; index++) {
-		/* MAC already in environment */
-		if (eth_env_get_enetaddr_by_index("eth", index, enetaddr))
-			continue;
-
-		for (i = 0; i < 6; i++)
-			enetaddr[i] = ((uint8_t *)&otp)[i + 6 * index];
-
-		if (!is_valid_ethaddr(enetaddr)) {
-			log_err("invalid MAC address %d in OTP %pM\n",
-				index, enetaddr);
-			return -EINVAL;
-		}
-		log_debug("OTP MAC address %d = %pM\n", index, enetaddr);
-		ret = eth_env_set_enetaddr_by_index("eth", index, enetaddr);
-		if (ret) {
-			log_err("Failed to set mac address %pM from OTP: %d\n",
-				enetaddr, ret);
-			return ret;
-		}
-	}
-
-	return 0;
-}
-
 __weak void stm32mp_misc_init(void)
 {
 }
diff --git a/arch/arm/mach-stm32mp/stm32mp2/cpu.c b/arch/arm/mach-stm32mp/stm32mp2/cpu.c
index 301e365cf4..9530aa8534 100644
--- a/arch/arm/mach-stm32mp/stm32mp2/cpu.c
+++ b/arch/arm/mach-stm32mp/stm32mp2/cpu.c
@@ -70,6 +70,7 @@ void enable_caches(void)
 int arch_misc_init(void)
 {
 	setup_serial_number();
+	setup_mac_address();
 
 	return 0;
 }