[PATCHv4] drivers: tee: i2c: support the NXP SE05x probe errata

[Jorge Ramirez-Ortiz <jorge@foundries.io>]

Early instantiation of this I2C device would lock up when being
probed.

 https://www.nxp.com/docs/en/errata/SE050_Erratasheet.pdf
 3.2.2
   In scenarios of detecting I2C ICs on the bus using an empty
   I2C frame containing only the address the SE050 will block
   the I2C bus.

Tested on STM32MP1

Signed-off-by: Jorge Ramirez-Ortiz <jorge@foundries.io>
Acked-by: Oleksandr Suvorov <oleksandr.suvorov@foundries.io>
---
 drivers/tee/optee/Kconfig | 14 +++++++++++++
 drivers/tee/optee/i2c.c   | 44 +++++++++++++++++++++++++++++++++++----
 2 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/drivers/tee/optee/Kconfig b/drivers/tee/optee/Kconfig
index d03028070b..a80ddaed2c 100644
--- a/drivers/tee/optee/Kconfig
+++ b/drivers/tee/optee/Kconfig
@@ -37,6 +37,20 @@ config OPTEE_TA_SCP03
 	help
 	  Enables support for controlling (enabling, provisioning) the
 	  Secure Channel Protocol 03 operation in the OP-TEE SCP03 TA.
+
+config TEE_I2C_NXP_SE05X_ERRATA
+	bool "Enable NXP SE05X Errata"
+       select TEE_I2C_NXP_SE05X_ERRATA_IN_BUS
+	default n
+	help
+         This config prevents the I2C trampoline driver from probing
+         on every transfer.
+
+config TEE_I2C_NXP_SE05X_ERRATA_IN_BUS
+	int "I2C bus where to apply the NXP SE05X errata"
+       depends on TEE_I2C_NXP_SE05X_ERRATA
+       default 0
+
 endmenu
 
 endif
diff --git a/drivers/tee/optee/i2c.c b/drivers/tee/optee/i2c.c
index ef4e10f991..e1b2534909 100644
--- a/drivers/tee/optee/i2c.c
+++ b/drivers/tee/optee/i2c.c
@@ -3,13 +3,18 @@
  * Copyright (c) 2020 Foundries.io Ltd
  */
 
+#define LOG_CATEGORY UCLASS_I2C
+
 #include <common.h>
 #include <dm.h>
 #include <i2c.h>
+#include <stdlib.h>
 #include <tee.h>
 #include "optee_msg.h"
 #include "optee_private.h"
 
+#define NXP_SE05X_ADDR 0x48
+
 static int check_xfer_flags(struct udevice *chip, uint tee_flags)
 {
 	uint flags;
@@ -30,6 +35,30 @@ static int check_xfer_flags(struct udevice *chip, uint tee_flags)
 	return 0;
 }
 
+static struct udevice *get_chip_dev(int bnum, int addr)
+{
+	struct udevice *chip;
+	struct udevice *bus;
+
+#if defined(CONFIG_TEE_I2C_NXP_SE05X_ERRATA)
+	if (bnum == CONFIG_TEE_I2C_NXP_SE05X_ERRATA_IN_BUS &&
+	    addr == NXP_SE05X_ADDR) {
+		if (uclass_get_device_by_seq(UCLASS_I2C, bnum, &bus))
+			return NULL;
+
+		if (i2c_get_chip(bus, addr, 0, &chip))
+			return NULL;
+
+		return chip;
+	}
+#endif
+
+	if (i2c_get_chip_for_busnum(bnum, addr, 0, &chip))
+		return NULL;
+
+	return chip;
+}
+
 void optee_suppl_cmd_i2c_transfer(struct optee_msg_arg *arg)
 {
 	const u8 attr[] = {
@@ -38,7 +67,8 @@ void optee_suppl_cmd_i2c_transfer(struct optee_msg_arg *arg)
 		OPTEE_MSG_ATTR_TYPE_RMEM_INOUT,
 		OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT,
 	};
-	struct udevice *chip_dev;
+	struct udevice *chip_dev = NULL;
+
 	struct tee_shm *shm;
 	u8 *buf;
 	int ret;
@@ -56,9 +86,9 @@ void optee_suppl_cmd_i2c_transfer(struct optee_msg_arg *arg)
 	if (!buf)
 		goto bad;
 
-	if (i2c_get_chip_for_busnum((int)arg->params[0].u.value.b,
-				    (int)arg->params[0].u.value.c,
-				    0, &chip_dev))
+	chip_dev = get_chip_dev((int)arg->params[0].u.value.b,
+				(int)arg->params[0].u.value.c);
+	if (!chip_dev)
 		goto bad;
 
 	if (check_xfer_flags(chip_dev, arg->params[1].u.value.a))
@@ -66,10 +96,16 @@ void optee_suppl_cmd_i2c_transfer(struct optee_msg_arg *arg)
 
 	switch (arg->params[0].u.value.a) {
 	case OPTEE_MSG_RPC_CMD_I2C_TRANSFER_RD:
+		log_debug("OPTEE_MSG_RPC_CMD_I2C_TRANSFER_RD %d\n",
+			  (size_t)arg->params[2].u.rmem.size);
+
 		ret = dm_i2c_read(chip_dev, 0, buf,
 				  (size_t)arg->params[2].u.rmem.size);
 		break;
 	case OPTEE_MSG_RPC_CMD_I2C_TRANSFER_WR:
+		log_debug("OPTEE_MSG_RPC_CMD_I2C_TRANSFER_WR %d\n",
+			  (size_t)arg->params[2].u.rmem.size);
+
 		ret = dm_i2c_write(chip_dev, 0, buf,
 				   (size_t)arg->params[2].u.rmem.size);
 		break;
-- 
2.34.1