From 8fedbbb946759fdf99f852cc00ea0bb155981447 Mon Sep 17 00:00:00 2001
From: XiaoDong Huang <derrick.huang@rock-chips.com>
Date: Wed, 16 Nov 2022 11:41:41 +0800
Subject: [PATCH] PM / devfreq: rockchip_bus: support parse soc-bus-table

Signed-off-by: XiaoDong Huang <derrick.huang@rock-chips.com>
Change-Id: I76ff30103cfa93289b9ecde0d95f42f960284e9b
---
 drivers/devfreq/rockchip_bus.c | 48 ++++++++++++++++++++++++++++++++--
 1 file changed, 46 insertions(+), 2 deletions(-)

diff --git a/drivers/devfreq/rockchip_bus.c b/drivers/devfreq/rockchip_bus.c
index 7032b4815c5a..88792d60a0b2 100644
--- a/drivers/devfreq/rockchip_bus.c
+++ b/drivers/devfreq/rockchip_bus.c
@@ -73,7 +73,9 @@ static int rockchip_bus_smc_config(struct rockchip_bus *bus)
 	struct device_node *np = dev->of_node;
 	struct device_node *child;
 	unsigned int enable_msk, bus_id, cfg;
-	int ret;
+	char *prp_name = "rockchip,soc-bus-table";
+	u32 *table = NULL;
+	int ret = 0, config_cnt, i;
 
 	for_each_available_child_of_node(np, child) {
 		ret = of_property_read_u32_index(child, "bus-id", 0,
@@ -108,7 +110,49 @@ static int rockchip_bus_smc_config(struct rockchip_bus *bus)
 		}
 	}
 
-	return 0;
+	config_cnt = of_property_count_u32_elems(np, prp_name);
+	if (config_cnt <= 0) {
+		return 0;
+	} else if (config_cnt % 3) {
+		dev_err(dev, "Invalid count of %s\n", prp_name);
+		return -EINVAL;
+	}
+
+	table = kmalloc_array(config_cnt, sizeof(u32), GFP_KERNEL);
+	if (!table)
+		return -ENOMEM;
+
+	ret = of_property_read_u32_array(np, prp_name, table, config_cnt);
+	if (ret) {
+		dev_err(dev, "get %s error\n", prp_name);
+		goto free_table;
+	}
+
+	/* table[3n]: bus_id
+	 * table[3n + 1]: config
+	 * table[3n + 2]: enable_mask
+	 */
+	for (i = 0; i < config_cnt; i += 3) {
+		bus_id = table[i];
+		cfg = table[i + 1];
+		enable_msk = table[i + 2];
+
+		if (!cfg) {
+			dev_info(dev, "cfg-val invalid in %s-%d\n", prp_name, bus_id);
+			continue;
+		}
+
+		ret = rockchip_sip_bus_smc_config(bus_id, cfg, enable_msk);
+		if (ret) {
+			dev_err(dev, "bus smc config error: %x!\n", ret);
+			goto free_table;
+		}
+	}
+
+free_table:
+	kfree(table);
+
+	return ret;
 }
 
 static int rockchip_bus_set_freq_table(struct rockchip_bus *bus)