Update fvmap.c

Author: Creeeeger

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

@@ -1,6 +1,7 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
+#include <linux/string.h>
 #include <linux/io.h>
 #include <linux/debugfs.h>
 #include <linux/uaccess.h>
@@ -13,6 +14,7 @@
 #include "ra.h"
 
 #define FVMAP_SIZE		(SZ_8K)
+#define FVMAP_EXTRA_SIZE	SZ_1K
 #define STEP_UV			(6250)
 
 void __iomem *fvmap_base;
@@ -21,6 +23,8 @@ void __iomem *sram_fvmap_base;
 static int init_margin_table[MAX_MARGIN_ID];
 static int volt_offset_percent = 0;
 static int percent_margin_table[MAX_MARGIN_ID];
+static size_t fvmap_alloc_offset;
+static size_t fvmap_alloc_limit;
 
 static int __init get_mif_volt(char *str)
 {
@@ -220,6 +224,75 @@ 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 int old_lv = fvmap_header->num_of_lv;
+	const 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;
+
+	if (old_lv >= new_lv)
+		return false;
+
+	old_rv = sram_base + fvmap_header->o_ratevolt;
+	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 = 754000;
+	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 names to reflect new size */
+	if (vclk)
+		vclk->num_of_lv = new_lv;
+
+	return true;
+}
+
 int fvmap_set_raw_voltage_table(unsigned int id, int uV)
 {
 	struct fvmap_header *fvmap_header;
@@ -306,47 +379,48 @@ static int get_vclk_id_from_margin_id(int margin_id)
 	int size = cmucal_get_list_size(ACPM_VCLK_TYPE);
 	int i;
 	struct vclk *vclk;
+	bool g3d_extended;
 
-	for (i = 0; i < size; i++) {
+		for (i = 0; i < size; i++) {
 		vclk = cmucal_get_node(ACPM_VCLK_TYPE | i);
 
 		if (vclk->margin_id == margin_id)
 			return i;
-	}
+		}
 
-	return -EINVAL;
+		return -EINVAL;
 }
 
 #define attr_percent(margin_id, type)								\
 static ssize_t show_##type##_percent								\
 (struct kobject *kobj, struct kobj_attribute *attr, char *buf)					\
 {												\
-	return snprintf(buf, PAGE_SIZE, "%d\n", percent_margin_table[margin_id]);		\
+		return snprintf(buf, PAGE_SIZE, "%d\n", percent_margin_table[margin_id]);		\
 }												\
 												\
 static ssize_t store_##type##_percent								\
 (struct kobject *kobj, struct kobj_attribute *attr, const char *buf, size_t count)		\
 {												\
-	int input, vclk_id;									\
+		int input, vclk_id;									\
 												\
-	if (!sscanf(buf, "%d", &input))								\
+		if (!sscanf(buf, "%d", &input))								\
 		return -EINVAL;									\
 												\
-	if (input < -100 || input > 100)							\
+		if (input < -100 || input > 100)							\
 		return -EINVAL;									\
 												\
-	vclk_id = get_vclk_id_from_margin_id(margin_id);					\
-	if (vclk_id == -EINVAL)									\
+		vclk_id = get_vclk_id_from_margin_id(margin_id);					\
+		if (vclk_id == -EINVAL)									\
 		return vclk_id;									\
-	percent_margin_table[margin_id] = input;						\
-	cal_dfs_set_volt_margin(vclk_id | ACPM_VCLK_TYPE, input);				\
+		percent_margin_table[margin_id] = input;						\
+		cal_dfs_set_volt_margin(vclk_id | ACPM_VCLK_TYPE, input);				\
 												\
-	return count;										\
+		return count;										\
 }												\
 												\
 static struct kobj_attribute type##_percent =							\
 __ATTR(type##_percent, 0600,									\
-	show_##type##_percent, store_##type##_percent)
+		show_##type##_percent, store_##type##_percent)
 
 attr_percent(MARGIN_MIF, mif_margin);
 attr_percent(MARGIN_INT, int_margin);
@@ -366,48 +440,50 @@ attr_percent(MARGIN_NPU, npu_margin);
 attr_percent(MARGIN_MFC, mfc_margin);
 
 static struct attribute *percent_margin_attrs[] = {
-	&mif_margin_percent.attr,
-	&int_margin_percent.attr,
-	&big_margin_percent.attr,
-	&mid_margin_percent.attr,
-	&lit_margin_percent.attr,
-	&g3d_margin_percent.attr,
-	&intcam_margin_percent.attr,
-	&cam_margin_percent.attr,
-	&disp_margin_percent.attr,
-	&cp_margin_percent.attr,
-	&fsys0_margin_percent.attr,
-	&aud_margin_percent.attr,
-	&iva_margin_percent.attr,
-	&score_margin_percent.attr,
-	&npu_margin_percent.attr,
-	&mfc_margin_percent.attr,
-	NULL,
+		&mif_margin_percent.attr,
+		&int_margin_percent.attr,
+		&big_margin_percent.attr,
+		&mid_margin_percent.attr,
+		&lit_margin_percent.attr,
+		&g3d_margin_percent.attr,
+		&intcam_margin_percent.attr,
+		&cam_margin_percent.attr,
+		&disp_margin_percent.attr,
+		&cp_margin_percent.attr,
+		&fsys0_margin_percent.attr,
+		&aud_margin_percent.attr,
+		&iva_margin_percent.attr,
+		&score_margin_percent.attr,
+		&npu_margin_percent.attr,
+		&mfc_margin_percent.attr,
+		NULL,
 };
 
 static const struct attribute_group percent_margin_group = {
-	.attrs = percent_margin_attrs,
+		.attrs = percent_margin_attrs,
 };
 
 static void fvmap_copy_from_sram(void __iomem *map_base, void __iomem *sram_base)
 {
-	volatile struct fvmap_header *fvmap_header, *header;
-	struct rate_volt_header *old, *new;
-	struct dvfs_table *old_param, *new_param;
-	struct clocks *clks;
-	struct pll_header *plls;
-	struct vclk *vclk;
-	unsigned int member_addr;
-	unsigned int blk_idx, param_idx;
-	int size, margin;
-	int i, j, k;
-
-	fvmap_header = map_base;
-	header = sram_base;
-
-	size = cmucal_get_list_size(ACPM_VCLK_TYPE);
-
-	for (i = 0; i < size; i++) {
+		volatile struct fvmap_header *fvmap_header, *header;
+		struct rate_volt_header *old, *new;
+		struct dvfs_table *old_param, *new_param;
+		struct clocks *clks;
+		struct pll_header *plls;
+		struct vclk *vclk;
+		bool g3d_extended;
+		unsigned int member_addr;
+		unsigned int blk_idx, param_idx;
+		unsigned short orig_o_ratevolt, orig_o_tables;
+		int size, margin;
+		int i, j, k;
+
+		fvmap_header = map_base;
+		header = sram_base;
+
+		size = cmucal_get_list_size(ACPM_VCLK_TYPE);
+
+		for (i = 0; i < size; i++) {
 		/* load fvmap info */
 		fvmap_header[i].dvfs_type = header[i].dvfs_type;
 		fvmap_header[i].num_of_lv = header[i].num_of_lv;
@@ -426,16 +502,26 @@ static void fvmap_copy_from_sram(void __iomem *map_base, void __iomem *sram_base
 		fvmap_header[i].o_members = header[i].o_members;
 		fvmap_header[i].o_ratevolt = header[i].o_ratevolt;
 		fvmap_header[i].o_tables = header[i].o_tables;
+		orig_o_ratevolt = fvmap_header[i].o_ratevolt;
+		orig_o_tables = fvmap_header[i].o_tables;
 
 		vclk = cmucal_get_node(ACPM_VCLK_TYPE | i);
 		if (vclk == NULL)
-			continue;
+		continue;
+
+		g3d_extended = false;
+
+		if (!strncmp(vclk->name, "dvfs_g3d", 8))
+		g3d_extended = fvmap_extend_g3d(&fvmap_header[i], map_base, sram_base, vclk);
 		pr_info("dvfs_type : %s - id : %x\n",
-			vclk->name, fvmap_header[i].dvfs_type);
+		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);
 
-		old = sram_base + fvmap_header[i].o_ratevolt;
+		if (g3d_extended)
+			old = map_base + fvmap_header[i].o_ratevolt;
+		else
+			old = sram_base + orig_o_ratevolt;
 		new = map_base + fvmap_header[i].o_ratevolt;
 
 		check_percent_margin(old, fvmap_header[i].num_of_lv);
@@ -478,7 +564,10 @@ static void fvmap_copy_from_sram(void __iomem *map_base, void __iomem *sram_base
 				volt_offset_percent);
 		}
 
-		old_param = sram_base + fvmap_header[i].o_tables;
+		if (g3d_extended)
+			old_param = map_base + fvmap_header[i].o_tables;
+		else
+			old_param = sram_base + orig_o_tables;
 		new_param = map_base + fvmap_header[i].o_tables;
 		for (j = 0; j < fvmap_header[i].num_of_lv; j++) {
 			for (k = 0; k < fvmap_header[i].num_of_members; k++) {
@@ -500,11 +589,16 @@ int fvmap_init(void __iomem *sram_base)
 {
 	void __iomem *map_base;
 	struct kobject *kobj;
+	size_t alloc_size;
+
+	alloc_size = FVMAP_SIZE + FVMAP_EXTRA_SIZE;
 
-	map_base = kzalloc(FVMAP_SIZE, GFP_KERNEL);
+	map_base = kzalloc(alloc_size, GFP_KERNEL);
 
 	fvmap_base = map_base;
 	sram_fvmap_base = sram_base;
+	fvmap_alloc_offset = FVMAP_SIZE;
+	fvmap_alloc_limit = alloc_size;
 	pr_info("%s:fvmap initialize %p\n", __func__, sram_base);
 	fvmap_copy_from_sram(map_base, sram_base);