clk: qcom: clk-cpu-osm: Allow overriding CPU frequency tables in DT

Sometimes, it may be desirable to use CPU frequency tables different
from the ones in the hardware's OSM LUTs. This commit adds support for
overriding each CPU's frequency table with a list of allowed frequencies
defined in the OSM driver's DT node.

Signed-off-by: Danny Lin <danny@kdrag0n.dev>
Signed-off-by: Adam W. Willis <return.of.octobot@gmail.com>

Documentation/devicetree/bindings/arm/msm/qcom,osm.txt

@@ -35,6 +35,13 @@ Properties:
 	Definition: List of phandles to devices that the OPP tables with the L3
 		    frequency and voltage mappings are loaded for.
 
+- qcom,cpufreq-table-XX
+	Usage: optional
+	Value type: <u32>
+	Definition: List of frequencies (in kHz) to expose in CPU XX's cpufreq table.
+		    All frequencies present in hardware will be exposed if this list
+		    is not present.
+
 Example:
 	clock_cpucc: qcom,cpucc {
 		compatible = "qcom,clk-cpu-osm";
@@ -48,4 +55,54 @@ Example:
 		l3-devs = <&phandle0 &phandle1 &phandle2>;
 
 		#clock-cells = <1>;
+		qcom,cpufreq-table-0 =
+			< 300000>,
+			< 403200>,
+			< 480000>,
+			< 576000>,
+			< 672000>,
+			< 768000>,
+			< 864000>,
+			< 979200>,
+			<1075200>,
+			<1171200>,
+			<1267200>;
+
+		qcom,cpufreq-table-4 =
+			< 576000>,
+			< 672000>,
+			< 768000>,
+			< 864000>,
+			< 960000>,
+			<1056000>,
+			<1152000>,
+			<1248000>,
+			<1344000>,
+			<1420800>,
+			<1497600>,
+			<1593600>,
+			<1689600>,
+			<1785600>,
+			<1862400>,
+			<1939200>,
+			<2016000>;
+
+		qcom,cpufreq-table-7 =
+			< 691200>,
+			< 768000>,
+			< 864000>,
+			< 940800>,
+			<1017600>,
+			<1113600>,
+			<1190400>,
+			<1286400>,
+			<1363200>,
+			<1459200>,
+			<1536000>,
+			<1632000>,
+			<1728000>,
+			<1824000>,
+			<1900800>,
+			<1977600>,
+			<2054400>;
 	};

drivers/clk/qcom/clk-cpu-osm.c

@@ -68,6 +68,7 @@ struct osm_entry {
 };
 
 struct clk_osm {
+	struct device *dev;
 	struct clk_hw hw;
 	struct osm_entry osm_table[OSM_TABLE_SIZE];
 	struct dentry *debugfs;
@@ -643,13 +644,30 @@ static unsigned int osm_cpufreq_get(unsigned int cpu)
 	return policy->freq_table[index].frequency;
 }
 
+static bool osm_dt_find_freq(u32 *of_table, int of_len, long frequency)
+{
+	int i;
+
+	if (!of_table)
+		return true;
+
+	for (i = 0; i < of_len; i++) {
+		if (frequency == of_table[i])
+			return true;
+	}
+
+	return false;
+}
+
 static int osm_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
 	struct cpufreq_frequency_table *table;
 	struct clk_osm *c, *parent;
 	struct clk_hw *p_hw;
-	int ret;
+	int ret, of_len;
 	unsigned int i;
+	u32 *of_table = NULL;
+	char tbl_name[] = "qcom,cpufreq-table-##";
 
 	c = osm_configure_policy(policy);
 	if (!c) {
@@ -666,6 +684,26 @@ static int osm_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	parent = to_clk_osm(p_hw);
 	c->vbase = parent->vbase;
 
+	snprintf(tbl_name, sizeof(tbl_name), "qcom,cpufreq-table-%d", policy->cpu);
+	if (of_find_property(parent->dev->of_node, tbl_name, &of_len) && of_len > 0) {
+		of_len /= sizeof(*of_table);
+
+		of_table = kcalloc(of_len, sizeof(*of_table), GFP_KERNEL);
+		if (!of_table) {
+			pr_err("failed to allocate DT frequency table memory for CPU%d\n",
+			       policy->cpu);
+			return -ENOMEM;
+		}
+
+		ret = of_property_read_u32_array(parent->dev->of_node, tbl_name,
+						 of_table, of_len);
+		if (ret) {
+			pr_err("failed to read DT frequency table for CPU%d, err=%d\n",
+			       policy->cpu, ret);
+			return ret;
+		}
+	}
+
 	table = kcalloc(parent->osm_table_size + 1, sizeof(*table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
@@ -686,6 +724,10 @@ static int osm_cpufreq_cpu_init(struct cpufreq_policy *policy)
 			table[i].frequency = (XO_RATE * lval) / 1000;
 		table[i].driver_data = table[i].frequency;
 
+		/* Ignore frequency if not present in DT table */
+		if (!osm_dt_find_freq(of_table, of_len, table[i].frequency))
+			table[i].frequency = CPUFREQ_ENTRY_INVALID;
+
 		if (core_count == SINGLE_CORE_COUNT)
 			table[i].frequency = CPUFREQ_ENTRY_INVALID;
 
@@ -715,9 +757,11 @@ static int osm_cpufreq_cpu_init(struct cpufreq_policy *policy)
 
 	cpumask_copy(policy->cpus, &c->related_cpus);
 
+	kfree(of_table);
 	return 0;
 
 err:
+	kfree(of_table);
 	kfree(table);
 	return ret;
 }
@@ -946,6 +990,7 @@ static int clk_osm_read_lut(struct platform_device *pdev, struct clk_osm *c)
 {
 	u32 data, src, lval, i, j = c->osm_table_size;
 
+	c->dev = &pdev->dev;
 	for (i = 0; i < c->osm_table_size; i++) {
 		data = clk_osm_read_reg(c, FREQ_REG + i * OSM_REG_SIZE);
 		src = ((data & GENMASK(31, 30)) >> 30);