add table mod back

Author: Creeeeger

drivers/soc/samsung/cal-if/cal-if.c

@@ -15,6 +15,7 @@
 #include "ra.h"
 #include "acpm_dvfs.h"
 #include "fvmap.h"
+#include "gpu_dvfs_overrides.h"
 #include "asv.h"
 
 #include "pmucal_system.h"
@@ -29,14 +30,39 @@
 
 static DEFINE_SPINLOCK(pmucal_cpu_lock);
 
+static bool cal_is_gpu_dvfs_id(unsigned int id)
+{
+	struct vclk *vclk;
+
+	vclk = cmucal_get_node(id);
+	if (!vclk || !vclk->name)
+		return false;
+
+	return !strcmp(vclk->name, "dvfs_g3d");
+}
+
 unsigned int cal_clk_is_enabled(unsigned int id)
 {
 	return 0;
 }
 
 unsigned long cal_dfs_get_max_freq(unsigned int id)
 {
-	return vclk_get_max_freq(id);
+	struct vclk *vclk;
+	unsigned long highest_override;
+
+	if (!cal_is_gpu_dvfs_id(id))
+		return vclk_get_max_freq(id);
+
+	if (!gpu_dvfs_has_overrides())
+		return vclk_get_max_freq(id);
+
+	highest_override = gpu_dvfs_override_highest_rate();
+	vclk = cmucal_get_node(id);
+	if (vclk && vclk->lut && highest_override > vclk->max_freq)
+		vclk->max_freq = highest_override;
+
+	return highest_override ? highest_override : vclk_get_max_freq(id);
 }
 
 unsigned long cal_dfs_get_min_freq(unsigned int id)
@@ -118,9 +144,62 @@ unsigned long cal_dfs_get_rate(unsigned int id)
 int cal_dfs_get_rate_table(unsigned int id, unsigned long *table)
 {
 	int ret;
+	struct vclk *vclk;
+	size_t override_idx;
 
 	ret = vclk_get_rate_table(id, table);
 
+	if (!cal_is_gpu_dvfs_id(id) || ret <= 0 || !gpu_dvfs_has_overrides())
+		return ret;
+
+	vclk = cmucal_get_node(id);
+	if (!vclk || !vclk->lut)
+		return ret;
+
+	if (ret > vclk->num_rates)
+		ret = vclk->num_rates;
+
+	for (override_idx = 0; override_idx < gpu_dvfs_override_count(); override_idx++) {
+		const struct gpu_dvfs_override_entry *entry;
+		bool found = false;
+		int insert_idx = vclk->num_rates;
+		int idx;
+
+		entry = gpu_dvfs_override_get(override_idx);
+		if (!entry)
+			continue;
+
+		for (idx = 0; idx < ret; idx++) {
+			if (table[idx] == entry->rate_khz) {
+				found = true;
+				break;
+			}
+		}
+
+		if (found)
+			continue;
+
+		for (idx = 0; idx < vclk->num_rates; idx++) {
+			if (vclk->lut[idx].rate == entry->rate_khz) {
+				insert_idx = idx;
+				break;
+			}
+
+			if (insert_idx == vclk->num_rates && vclk->lut[idx].rate < entry->rate_khz)
+				insert_idx = idx;
+		}
+
+		if (insert_idx > ret)
+			insert_idx = ret;
+
+		if (ret < vclk->num_rates) {
+			for (idx = ret; idx > insert_idx; idx--)
+				table[idx] = table[idx - 1];
+			ret++;
+		}
+
+		table[insert_idx] = entry->rate_khz;
+	}
 	return ret;
 }
 
@@ -339,7 +418,99 @@ extern int cal_is_lastcore_detecting(unsigned int cpu)
 
 int cal_dfs_get_asv_table(unsigned int id, unsigned int *table)
 {
-	return fvmap_get_voltage_table(id, table);
+	int entries;
+	struct vclk *vclk;
+	size_t override_count = 0;
+	size_t override_idx;
+	struct gpu_override_plan {
+		const struct gpu_dvfs_override_entry *entry;
+		int index;
+	} *plans = NULL;
+
+	entries = fvmap_get_voltage_table(id, table);
+
+	if (!cal_is_gpu_dvfs_id(id) || entries <= 0 || !gpu_dvfs_has_overrides())
+		return entries;
+
+	vclk = cmucal_get_node(id);
+	if (!vclk || !vclk->lut)
+		return entries;
+
+	if (entries > vclk->num_rates)
+		entries = vclk->num_rates;
+
+	override_count = gpu_dvfs_override_count();
+	plans = kcalloc(override_count, sizeof(*plans), GFP_KERNEL);
+	if (!plans)
+		return entries;
+
+	for (override_idx = 0; override_idx < override_count; override_idx++) {
+		const struct gpu_dvfs_override_entry *entry;
+		int idx;
+
+		entry = gpu_dvfs_override_get(override_idx);
+		if (!entry)
+			continue;
+
+		for (idx = 0; idx < vclk->num_rates; idx++) {
+			if (vclk->lut[idx].rate == entry->rate_khz) {
+				plans[override_idx].entry = entry;
+				plans[override_idx].index = idx;
+				break;
+			}
+		}
+	}
+
+	/* sort overrides by target index descending to keep shifts safe */
+	for (override_idx = 0; override_idx < override_count; override_idx++) {
+		size_t other;
+
+		if (!plans[override_idx].entry)
+			continue;
+
+		for (other = override_idx + 1; other < override_count; other++) {
+			if (!plans[other].entry)
+				continue;
+
+			if (plans[other].index > plans[override_idx].index) {
+				struct gpu_override_plan tmp = plans[override_idx];
+				plans[override_idx] = plans[other];
+				plans[other] = tmp;
+			}
+		}
+	}
+
+	for (override_idx = 0; override_idx < override_count; override_idx++) {
+		const struct gpu_dvfs_override_entry *entry = plans[override_idx].entry;
+		int idx = plans[override_idx].index;
+		int shift;
+
+		if (!entry)
+			continue;
+
+		if (idx > entries)
+			idx = entries;
+
+		if (idx >= entries) {
+			if (entries < vclk->num_rates) {
+				table[entries] = entry->volt_uv;
+				entries++;
+			}
+			continue;
+		}
+
+		if (entries < vclk->num_rates) {
+			for (shift = entries; shift > idx; shift--)
+				table[shift] = table[shift - 1];
+			entries++;
+		}
+
+		table[idx] = entry->volt_uv;
+	}
+
+	kfree(plans);
+
+	return entries;
 }
 
 void cal_dfs_set_volt_margin(unsigned int id, int volt)

drivers/soc/samsung/cal-if/fvmap.c

@@ -13,6 +13,7 @@
 
 #include "cmucal.h"
 #include "fvmap.h"
+#include "gpu_dvfs_overrides.h"
 #include "ra.h"
 #include "vclk.h"
 
@@ -26,6 +27,37 @@ static int init_margin_table[MAX_MARGIN_ID];
 static int volt_offset_percent = 0;
 static int percent_margin_table[MAX_MARGIN_ID];
 
+#define G3D_MANUAL_RATE(_mhz, _uv) {.rate = (_mhz) * 1000U, .volt = (_uv)}
+
+static const struct rate_volt g3d_manual_ratevolt[] = {
+    G3D_MANUAL_RATE(754,  800000),
+    G3D_MANUAL_RATE(702,  775000),
+    G3D_MANUAL_RATE(650,  750000),
+    G3D_MANUAL_RATE(572,  725000),
+    G3D_MANUAL_RATE(433,  700000),
+    G3D_MANUAL_RATE(377,  675000),
+    G3D_MANUAL_RATE(325,  650000),
+    G3D_MANUAL_RATE(260,  625000),
+    G3D_MANUAL_RATE(200,  600000),
+    G3D_MANUAL_RATE(156,  575000),
+    G3D_MANUAL_RATE(100,  550000),
+};
+
+static size_t
+fvmap_ratevolt_capacity(const volatile struct fvmap_header *header) {
+    size_t capacity = header->num_of_lv;
+
+    if (header->o_tables > header->o_ratevolt) {
+        size_t ratevolt_capacity =
+            (header->o_tables - header->o_ratevolt) / sizeof(struct rate_volt);
+
+        if (ratevolt_capacity && ratevolt_capacity < capacity)
+            capacity = ratevolt_capacity;
+    }
+
+    return capacity;
+}
+
 static size_t
 fvmap_calculate_initial_usage(const volatile struct fvmap_header *header,
                               int num_of_vclks) {
@@ -50,6 +82,35 @@ fvmap_calculate_initial_usage(const volatile struct fvmap_header *header,
     return ALIGN(max_offset, sizeof(u32));
 }
 
+static void fvmap_apply_gpu_manual_table(volatile struct fvmap_header *header,
+                                         struct rate_volt_header *rate_table,
+                                         struct vclk *vclk) {
+    size_t manual_count = ARRAY_SIZE(g3d_manual_ratevolt);
+    size_t capacity = fvmap_ratevolt_capacity(header);
+    size_t idx;
+
+    if (capacity < manual_count) {
+        pr_warn("  G3D manual table truncated to %zu entries (capacity %zu)\n",
+                capacity, manual_count);
+        manual_count = capacity;
+    }
+
+    if (!manual_count)
+        return;
+
+    memcpy(rate_table->table, g3d_manual_ratevolt,
+           manual_count * sizeof(struct rate_volt));
+    header->num_of_lv = manual_count;
+
+    if (vclk && vclk->lut && vclk->num_rates < manual_count)
+        vclk->num_rates = manual_count;
+
+    if (vclk && vclk->lut) {
+        for (idx = 0; idx < manual_count && idx < vclk->num_rates; idx++)
+            vclk->lut[idx].rate = g3d_manual_ratevolt[idx].rate;
+    }
+}
+
 static int __init get_mif_volt(char *str) {
     int volt;
 
@@ -451,11 +512,60 @@ static void fvmap_copy_from_sram(void __iomem *map_base,
 
         fw_lv = header[i].num_of_lv;
 
+        if (!strcmp(vclk->name, "dvfs_g3d")) {
+            size_t manual_lv = ARRAY_SIZE(g3d_manual_ratevolt);
+            size_t capacity = fvmap_ratevolt_capacity(&header[i]);
+
+            if (manual_lv > capacity) {
+                size_t ratevolt_bytes = manual_lv * sizeof(struct rate_volt);
+                size_t table_bytes = manual_lv * fvmap_header[i].num_of_members;
+                size_t new_ratevolt_offset =
+                    ALIGN(next_free_offset, sizeof(struct rate_volt));
+                size_t new_tables_offset =
+                    ALIGN(new_ratevolt_offset + ratevolt_bytes, sizeof(u32));
+                size_t new_end = new_tables_offset + table_bytes;
+
+                if (new_end > FVMAP_SIZE) {
+                    pr_err("  G3D: unable to extend manual table, "
+                           "need %zu bytes, limit %lu – truncating\n",
+                           new_end, FVMAP_SIZE);
+                    manual_lv = capacity;
+                } else {
+                    pr_info("  G3D: relocating rate/volt tables to 0x%zx "
+                            "to fit %zu entries\n",
+                            new_ratevolt_offset, manual_lv);
+                    fvmap_header[i].o_ratevolt = new_ratevolt_offset;
+                    fvmap_header[i].o_tables = new_tables_offset;
+                    next_free_offset = new_end;
+                    capacity = manual_lv;
+                }
+            }
+
+            if (manual_lv > capacity) {
+                pr_warn("  G3D: manual table has %zu entries, "
+                        "FW/capacity only %zu – truncating!\n",
+                        manual_lv, capacity);
+                manual_lv = capacity;
+            }
+
+            pr_info("  G3D: using %zu levels from manual table "
+                    "(FW advertised %u)\n",
+                    manual_lv, fw_lv);
+
+            fvmap_header[i].num_of_lv = manual_lv;
+        }
+
         pr_info("dvfs_type : %s - id : %x\n", vclk->name,
                 fvmap_header[i].dvfs_type);
         pr_info("  num_of_lv      : %d\n", fvmap_header[i].num_of_lv);
         pr_info("  num_of_members : %d\n", fvmap_header[i].num_of_members);
 
+        if (!strcmp(vclk->name, "dvfs_g3d")) {
+            pr_info("  G3D init level : %d\n", fvmap_header[i].init_lv);
+            pr_info("  G3D volt_offset_percent : %d\n", volt_offset_percent);
+            pr_info("  Using manual G3D rate/volt table\n");
+        }
+
         old = sram_base + header[i].o_ratevolt;
         new = map_base + fvmap_header[i].o_ratevolt;
 
@@ -504,6 +614,10 @@ static void fvmap_copy_from_sram(void __iomem *map_base,
                     new->table[j].volt, volt_offset_percent);
         }
 
+        if (!strcmp(vclk->name, "dvfs_g3d")) {
+            fvmap_apply_gpu_manual_table(&fvmap_header[i], new, vclk);
+        }
+
         for (j = 0; j < fvmap_header[i].num_of_lv; j++) {
             pr_info("  lv : [%7d], volt = %d uV (%d %%) \n", new->table[j].rate,
                     new->table[j].volt, volt_offset_percent);
@@ -526,6 +640,18 @@ static void fvmap_copy_from_sram(void __iomem *map_base,
             }
         }
 
+        if (!strcmp(vclk->name, "dvfs_g3d")) {
+            size_t manual_lv = fvmap_header[i].num_of_lv;
+
+            if (manual_lv > fw_lv && fw_lv) {
+                size_t stride = fvmap_header[i].num_of_members;
+
+                for (j = fw_lv; j < manual_lv; j++) {
+                    memcpy(&new_param->val[stride * j],
+                           &new_param->val[stride * (fw_lv - 1)], stride);
+                }
+            }
+        }
     }
 }