Update fvmap.c

Author: Creeeeger

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

@@ -220,6 +220,97 @@ static int __init get_percent_margin_volt(char *str)
 }
 early_param("volt_offset_percent", get_percent_margin_volt);
 
+static void *fvmap_alloc_extra(size_t size, size_t align)
+{
+	void *ptr;
+
+	fvmap_alloc_offset = ALIGN(fvmap_alloc_offset, align);
+	if (fvmap_alloc_offset + size > fvmap_alloc_limit)
+		return NULL;
+
+	ptr = fvmap_base + fvmap_alloc_offset;
+	fvmap_alloc_offset += size;
+
+	return ptr;
+}
+
+static bool fvmap_extend_g3d(struct fvmap_header *fvmap_header,
+                             void __iomem *map_base, void __iomem *sram_base,
+                             struct vclk *vclk)
+{
+        const unsigned int oc_rate = 754000;
+        const int old_lv = fvmap_header->num_of_lv;
+        int new_lv = 13;
+        struct rate_volt_header *old_rv, *new_rv;
+        struct dvfs_table *old_tbl, *new_tbl;
+        int members = fvmap_header->num_of_members;
+        bool have_oc;
+
+        old_rv = sram_base + fvmap_header->o_ratevolt;
+        have_oc = old_lv > 0 && old_rv->table[0].rate == oc_rate;
+
+        if (old_lv >= new_lv && have_oc)
+                return false;
+
+        if (old_lv >= new_lv)
+                new_lv = old_lv + 1;
+
+        new_rv = fvmap_alloc_extra(sizeof(struct rate_volt) * new_lv,
+                                __alignof__(struct rate_volt));
+        if (!new_rv) {
+		pr_err("[%s] g3d: no space for extended rate/volt table\n",
+			__func__);
+		return false;
+	}
+
+        /* insert OC level at the top and shift existing entries down */
+        new_rv->table[0].rate = oc_rate;
+        new_rv->table[0].volt = old_rv->table[0].volt;
+        memcpy(&new_rv->table[1], &old_rv->table[0],
+               sizeof(struct rate_volt) * old_lv);
+
+	old_tbl = sram_base + fvmap_header->o_tables;
+	new_tbl = fvmap_alloc_extra(members * new_lv, 1);
+	if (!new_tbl) {
+		pr_err("[%s] g3d: no space for extended dvfs table\n",
+			__func__);
+		return false;
+	}
+
+	memcpy(&new_tbl->val[0], &old_tbl->val[0], members);
+	memcpy(&new_tbl->val[members], &old_tbl->val[0], members * old_lv);
+
+        fvmap_header->o_ratevolt = (unsigned short)((void *)new_rv - map_base);
+        fvmap_header->o_tables = (unsigned short)((void *)new_tbl - map_base);
+        fvmap_header->num_of_lv = new_lv;
+        if (fvmap_header->init_lv < new_lv)
+                fvmap_header->init_lv += 1;
+
+        pr_info("dvfs_g3d: extended FV table %d -> %d levels (OC 754MHz)\n",
+                old_lv, new_lv);
+
+        /* update vclk LUT to expose the new OC rate to CAL/clients */
+        if (vclk && vclk->lut) {
+                int lut_sz = vclk->num_rates;
+                int copy_lv = min(new_lv - 1, old_lv);
+                int idx;
+
+                if (lut_sz < new_lv)
+                        lut_sz = new_lv;
+
+                for (idx = min(lut_sz - 1, copy_lv); idx > 0; idx--)
+                        vclk->lut[idx].rate = vclk->lut[idx - 1].rate;
+
+                vclk->lut[0].rate = oc_rate;
+                vclk->num_rates = max(vclk->num_rates, (unsigned int)new_lv);
+                vclk->max_freq = max(vclk->max_freq, oc_rate);
+                vclk->min_freq = min(vclk->min_freq,
+                                     new_rv->table[new_lv - 1].rate);
+        }
+
+        return true;
+}
+
 int fvmap_set_raw_voltage_table(unsigned int id, int uV)
 {
 	struct fvmap_header *fvmap_header;