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

[Jorge Ramirez-Ortiz]

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

Signed-off-by: Jorge Ramirez-Ortiz <jorge at 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..05dfe2c9a8 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 y
+	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..a3ea34d4a2 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 (IS_ENABLED(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;
+		}
+	}
+
+	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