[PATCH u-boot-mvebu v3 13/18] gpio: turris_omnia_mcu: Update firmware features reading

[Marek Behún]

Update firmware features reading to try reading 32 bits of features and
fallback to reading 16 bits.

Signed-off-by: Marek Behún <kabel at kernel.org>
---
 drivers/gpio/turris_omnia_mcu.c | 32 ++++++++++++++++++++++----------
 1 file changed, 22 insertions(+), 10 deletions(-)

diff --git a/drivers/gpio/turris_omnia_mcu.c b/drivers/gpio/turris_omnia_mcu.c
index c441d07d69..40ced261e3 100644
--- a/drivers/gpio/turris_omnia_mcu.c
+++ b/drivers/gpio/turris_omnia_mcu.c
@@ -10,7 +10,7 @@
 #include <linux/log2.h>
 
 struct turris_omnia_mcu_info {
-	u16 features;
+	u32 features;
 };
 
 static int turris_omnia_mcu_get_function(struct udevice *dev, uint offset)
@@ -228,25 +228,37 @@ static int turris_omnia_mcu_probe(struct udevice *dev)
 {
 	struct turris_omnia_mcu_info *info = dev_get_plat(dev);
 	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
-	u16 val;
+	u32 dword;
+	u16 word;
 	int ret;
 
-	ret = dm_i2c_read(dev, CMD_GET_STATUS_WORD, (void *)&val, sizeof(val));
+	ret = dm_i2c_read(dev, CMD_GET_STATUS_WORD, (void *)&word, sizeof(word));
 	if (ret < 0) {
 		printf("Error: turris_omnia_mcu CMD_GET_STATUS_WORD failed: %d\n",
 		       ret);
 		return ret;
 	}
 
-	if (le16_to_cpu(val) & STS_FEATURES_SUPPORTED) {
-		ret = dm_i2c_read(dev, CMD_GET_FEATURES, (void *)&val,
-				  sizeof(val));
+	if (le16_to_cpu(word) & STS_FEATURES_SUPPORTED) {
+		/* try read 32-bit features */
+		ret = dm_i2c_read(dev, CMD_GET_FEATURES, (void *)&dword,
+				  sizeof(dword));
 		if (ret < 0) {
-			printf("Error: turris_omnia_mcu CMD_GET_FEATURES failed: %d\n",
-			       ret);
-			return ret;
+			/* try read 16-bit features */
+			ret = dm_i2c_read(dev, CMD_GET_FEATURES, (void *)&word,
+					  sizeof(word));
+			if (ret < 0) {
+				printf("Error: turris_omnia_mcu CMD_GET_FEATURES failed: %d\n",
+				       ret);
+				return ret;
+			}
+
+			info->features = le16_to_cpu(word);
+		} else {
+			info->features = le32_to_cpu(dword);
+			if (info->features & FEAT_FROM_BIT_16_INVALID)
+				info->features &= GENMASK(15, 0);
 		}
-		info->features = le16_to_cpu(val);
 	}
 
 	uc_priv->bank_name = "mcu_";
-- 
2.43.2