#include <stdio.h>

#define max(x, y)   \
	(((x) > (y)) ? (x) : (y))
#define min(x, y)   \
	(((x) < (y)) ? (x) : (y))
#define clamp(val, lo, hi)  \
	((val) >= (hi) ? (hi) : ((val) <= (lo) ? (lo) : (val)))

#define PROC_NAME	"proc"
#define SRAM_NAME	"sram" 
#define PROC_ID		0
#define SRAM_ID		1
#define proc_reg	PROC_ID
#define sram_reg	SRAM_ID
#define LIST_CSIZE	8
const char *reg_name_list[] = {
	PROC_NAME, SRAM_NAME
};

const int proc_volt_list[LIST_CSIZE] = {
	1310000, 1250000, 1200000, 1150000,
	1100000, 1050000, 1000000,  950000
};

const int sram_volt_list[LIST_CSIZE] = {
	1400000, 1350000, 1300000, 1250000,
	1200000, 1150000, 1100000, 1050000
};
int proc_volt = -1;
int sram_volt = -1;

struct mtk_cpufreq_platform_data {
	int min_volt_shift;
	int max_volt_shift;
	int proc_max_volt;
	int sram_min_volt;
	int sram_max_volt;
};

struct mtk_cpu_dvfs_info {
	const struct mtk_cpufreq_platform_data *soc_data;
};

static int regulator_get_voltage(int reg_id)
{
	if (reg_id == PROC_ID)
		return proc_volt;
	else if (reg_id == SRAM_ID)
		return sram_volt;

	printf("### [%s] reg_id: %d not available\n", __func__, reg_id);
	return -1;
}

static int check_sram_valid(int volt)
{
	int found = 0;

	for (int i = 0; i < LIST_CSIZE; ++i)
		if (volt == sram_volt_list[i]) {
			found = 1;
			break;
		}

	return found;
}

static int regulator_set_voltage(int reg_id, int min_uv, int max_uv)
{
	int ret = 0;

	if (reg_id == PROC_ID) {
		proc_volt = min_uv;
		printf("### [%s] set %s volt %d/%d uV\n",
			__func__, reg_name_list[reg_id], min_uv, max_uv);
	} else if (reg_id == SRAM_ID) {
		if (check_sram_valid(min_uv)) {
			sram_volt = min_uv;
			printf("### [%s] set %s volt %d/%d uV\n",
				__func__, reg_name_list[reg_id], min_uv, max_uv);
		} else {
			ret = -1;
			printf("!!! [%s] set %s volt %d uV is not valid\n",
				__func__, reg_name_list[reg_id], min_uv);
		}
	}
	else
		ret = -1;
	
	return ret;
}

static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
					int new_vproc)
{
	const struct mtk_cpufreq_platform_data *soc_data = info->soc_data;
//	struct regulator *proc_reg = info->proc_reg;
//	struct regulator *sram_reg = info->sram_reg;
	int pre_vproc, pre_vsram, new_vsram, vsram, vproc, ret;
//	int retry = info->vtrack_max;

	pre_vproc = regulator_get_voltage(proc_reg);
//	if (pre_vproc < 0) {
//		dev_err(info->cpu_dev,
//			"invalid Vproc value: %d\n", pre_vproc);
//		return pre_vproc;
//	}

	pre_vsram = regulator_get_voltage(sram_reg);
//	if (pre_vsram < 0) {
//		dev_err(info->cpu_dev, "invalid Vsram value: %d\n", pre_vsram);
//		return pre_vsram;
//	}

	new_vsram = clamp(new_vproc + soc_data->min_volt_shift,
			  soc_data->sram_min_volt, soc_data->sram_max_volt);

	printf("## [%s] target vproc: %d; vsram: %d\n", __func__, new_vproc, new_vsram);
	do {
		if (pre_vproc <= new_vproc) {
			vsram = clamp(pre_vproc + soc_data->max_volt_shift,
				      soc_data->sram_min_volt, new_vsram);
			ret = regulator_set_voltage(sram_reg, vsram,
						    soc_data->sram_max_volt);

			if (ret)
				return ret;

			if (vsram == soc_data->sram_max_volt ||
			    new_vsram == soc_data->sram_min_volt)
				vproc = new_vproc;
			else
				vproc = vsram - soc_data->min_volt_shift;

			ret = regulator_set_voltage(proc_reg, vproc,
						    soc_data->proc_max_volt);
			if (ret) {
				regulator_set_voltage(sram_reg, pre_vsram,
						      soc_data->sram_max_volt);
				return ret;
			}
		} else if (pre_vproc > new_vproc) {
			vproc = max(new_vproc,
				    pre_vsram - soc_data->max_volt_shift);
			ret = regulator_set_voltage(proc_reg, vproc,
						    soc_data->proc_max_volt);
			if (ret)
				return ret;

			if (vproc == new_vproc)
				vsram = new_vsram;
			else
				vsram = max(new_vsram,
					    vproc + soc_data->min_volt_shift);

			ret = regulator_set_voltage(sram_reg, vsram,
						    soc_data->sram_max_volt);
			if (ret) {
				regulator_set_voltage(proc_reg, pre_vproc,
						      soc_data->proc_max_volt);
				return ret;
			}
		}

		pre_vproc = vproc;
		pre_vsram = vsram;

//		if (--retry < 0) {
//			dev_err(info->cpu_dev,
//				"over loop count, failed to set voltage\n");
//			return -EINVAL;
//		}
	} while (vproc != new_vproc || vsram != new_vsram);

	return 0;
}

int main(void)
{
	struct mtk_cpufreq_platform_data soc_data = {
		.min_volt_shift = 100000,
		.max_volt_shift = 200000,
		.proc_max_volt = 1310000,
		.sram_min_volt = 0,
		.sram_max_volt = 1350000,
	};

	struct mtk_cpu_dvfs_info info = {
		.soc_data = &soc_data,
	};

	proc_volt = 950000;
	sram_volt = 1050000;

	printf("[%s] init setting: \n", __func__);
	printf("init proc_volt: %d\n", proc_volt);
	printf("init sram_volt: %d\n", sram_volt);
	printf("min_volt_shift: %d\n", soc_data.min_volt_shift);
	printf("max_volt_shift: %d\n", soc_data.max_volt_shift);
	printf("proc_max_volt : %d\n", soc_data.proc_max_volt );
	printf("sram_min_volt : %d\n", soc_data.sram_min_volt );
	printf("sram_max_volt : %d\n", soc_data.sram_max_volt );

	printf("\n");
	printf("##### Test Start #####\n");
	int fail_count = 0;
	for (int i = 0; i < LIST_CSIZE; ++i) {
		for (int j = 0; j < LIST_CSIZE; ++j) {
			if (i == j)
				continue;
			printf("opp[%d]->[%d]\n", i, j);
			fail_count += mtk_cpufreq_voltage_tracking(&info, proc_volt_list[i]);
			fail_count += mtk_cpufreq_voltage_tracking(&info, proc_volt_list[j]);
			printf("\n");
		}
	}

	printf("##### Test Result: %s. #####\n", fail_count ? "Failed" : "PASS");
	return 0;
}