[PATCH v1] nvmem: bsec.c: Use SMC_READ_OTP in stm32_bsec_read_mac()

[Oleksij Rempel]

From: David Jander <david at protonic.nl>

TF-A (version >= v2.4?) apparently does not copy all of OTP to shadow
registers so reading the MAC address area will read all-zeroes.
Make sure to read the _real_ OTP register in this case.

Signed-off-by: David Jander <david at protonic.nl>
Signed-off-by: Oleksij Rempel <o.rempel at pengutronix.de>
---
 drivers/nvmem/bsec.c | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/drivers/nvmem/bsec.c b/drivers/nvmem/bsec.c
index d9b38c8414..a31eff7358 100644
--- a/drivers/nvmem/bsec.c
+++ b/drivers/nvmem/bsec.c
@@ -82,20 +82,27 @@ static void stm32_bsec_set_unique_machine_id(struct regmap *map)
 	machine_id_set_hashable(unique_id, sizeof(unique_id));
 }
 
-static int stm32_bsec_read_mac(struct regmap *map, int offset, u8 *mac)
+static int stm32_bsec_read_mac(struct bsec_priv *priv, int offset, u8 *mac)
 {
-	u8 res[8];
+	u32 val[2];
 	int ret;
 
-	ret = regmap_bulk_read(map, offset * 4, res, 8);
+	/* Some TF-A does not copy all of OTP into shadow registers, so make
+	 * sure we read the _real_ OTP bits here.
+	 */
+	ret = bsec_smc(priv, BSEC_SMC_READ_OTP, offset * 4, 0, &val[0]);
+	if (ret)
+		return ret;
+	ret = bsec_smc(priv, BSEC_SMC_READ_OTP, offset * 4 + 4, 0, &val[1]);
 	if (ret)
 		return ret;
 
-	memcpy(mac, res, ETH_ALEN);
+	memcpy(mac, val, ETH_ALEN);
 	return 0;
 }
 
-static void stm32_bsec_init_dt(struct device_d *dev, struct regmap *map)
+static void stm32_bsec_init_dt(struct bsec_priv *priv, struct device_d *dev,
+			       struct regmap *map)
 {
 	struct device_node *node = dev->device_node;
 	struct device_node *rnode;
@@ -118,7 +125,7 @@ static void stm32_bsec_init_dt(struct device_d *dev, struct regmap *map)
 	rnode = of_find_node_by_phandle(phandle);
 	offset = be32_to_cpup(prop++);
 
-	ret = stm32_bsec_read_mac(map, offset, mac);
+	ret = stm32_bsec_read_mac(priv, offset, mac);
 	if (ret) {
 		dev_warn(dev, "error setting MAC address: %s\n", strerror(-ret));
 		return;
@@ -159,7 +166,7 @@ static int stm32_bsec_probe(struct device_d *dev)
 	if (IS_ENABLED(CONFIG_MACHINE_ID))
 		stm32_bsec_set_unique_machine_id(map);
 
-	stm32_bsec_init_dt(dev, map);
+	stm32_bsec_init_dt(priv, dev, map);
 
 	return 0;
 }
-- 
2.30.2