From 1a5b21655b941a5050b67b816fd910b2d69d0bab Mon Sep 17 00:00:00 2001
From: Ye Zhang <ye.zhang@rock-chips.com>
Date: Tue, 7 Jan 2025 11:33:45 +0800
Subject: [PATCH] gpio: rockchip: Fix rk3506 gpio4a0~4a5 mapping issue

Signed-off-by: Ye Zhang <ye.zhang@rock-chips.com>
Change-Id: Ic34079132f95e93b9a0777d6d1ceabee0e566026
---
 drivers/gpio/gpio-rockchip.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/gpio/gpio-rockchip.c b/drivers/gpio/gpio-rockchip.c
index caaf7706a37d..d38553dfad26 100644
--- a/drivers/gpio/gpio-rockchip.c
+++ b/drivers/gpio/gpio-rockchip.c
@@ -94,6 +94,13 @@ static const struct rockchip_gpio_regs gpio_regs_v2 = {
 	.version_id = 0x78,
 };
 
+static enum rockchip_pinctrl_type chip_type;
+
+static inline bool is_rk3506_bank4(struct rockchip_pin_bank *bank)
+{
+	return IS_ENABLED(CONFIG_CPU_RK3506) && chip_type == RK3506 && bank->bank_num == 4;
+}
+
 static inline void gpio_writel_v2(u32 val, void __iomem *reg)
 {
 	writel((val & 0xffff) | 0xffff0000, reg);
@@ -110,6 +117,14 @@ static inline void rockchip_gpio_writel(struct rockchip_pin_bank *bank,
 {
 	void __iomem *reg = bank->reg_base + offset;
 
+	if (is_rk3506_bank4(bank)) {
+		u32 tmp = value & 0x3f;
+
+		value &= 0xffffffc0;
+		value |= (tmp >> 1) & 0x15;
+		value |= (tmp << 1) & 0x2a;
+	}
+
 	if (bank->gpio_type >= GPIO_TYPE_V2)
 		gpio_writel_v2(value, reg);
 	else
@@ -127,6 +142,14 @@ static inline u32 rockchip_gpio_readl(struct rockchip_pin_bank *bank,
 	else
 		value = readl(reg);
 
+	if (is_rk3506_bank4(bank)) {
+		u32 tmp = value & 0x3f;
+
+		value &= 0xffffffc0;
+		value |= (tmp >> 1) & 0x15;
+		value |= (tmp << 1) & 0x2a;
+	}
+
 	return value;
 }
 
@@ -137,6 +160,9 @@ static inline void rockchip_gpio_writel_bit(struct rockchip_pin_bank *bank,
 	void __iomem *reg = bank->reg_base + offset;
 	u32 data;
 
+	if (is_rk3506_bank4(bank) && bit < 6)
+		bit ^= 0x1;
+
 	if (bank->gpio_type >= GPIO_TYPE_V2) {
 		if (value)
 			data = BIT(bit % 16) | BIT(bit % 16 + 16);
@@ -158,6 +184,9 @@ static inline u32 rockchip_gpio_readl_bit(struct rockchip_pin_bank *bank,
 	void __iomem *reg = bank->reg_base + offset;
 	u32 data;
 
+	if (is_rk3506_bank4(bank) && bit < 6)
+		bit ^= 0x1;
+
 	if (bank->gpio_type >= GPIO_TYPE_V2) {
 		data = readl(bit >= 16 ? reg + 0x4 : reg);
 		data >>= bit % 16;
@@ -709,6 +738,9 @@ rockchip_gpio_find_bank(struct pinctrl_dev *pctldev, int id)
 	int i, found = 0;
 
 	info = pinctrl_dev_get_drvdata(pctldev);
+	if (IS_ENABLED(CONFIG_CPU_RK3506))
+		chip_type = info->ctrl->type;
+
 	bank = info->ctrl->pin_banks;
 	for (i = 0; i < info->ctrl->nr_banks; i++, bank++) {
 		if (bank->bank_num == id) {