text: #include "pwrcal-env.h"
  #include "pwrcal-pmu.h"
  #include "pwrcal-clk.h"
  #include "pwrcal-vclk.h"
  #include "pwrcal-dfs.h"
  
  
  
  unsigned int dfs_set_rate_switch(unsigned int rate_from,
  					unsigned int rate_to,
  					struct dfs_table *table)
  {
  	unsigned int rate_max;
  	int i;
  
  	rate_max = (rate_from > rate_to) ? rate_from : rate_to;
  
  	for (i = 0; i < table->num_of_switches; i++) {
  		if (rate_max >= table->switches[i].switch_rate) {
  			if (is_div(table->switch_src_div))
  				if (pwrcal_div_set_ratio(
  					table->switch_src_div,
  					table->switches[i].div_value + 1))
  					goto errorout;
  
  			if (is_mux(table->switch_src_mux))
  				if (pwrcal_mux_set_src(
  					table->switch_src_mux,
  					table->switches[i].mux_value))
  					goto errorout;
  
  			return table->switches[i].switch_rate;
  		}
  	}
  
  	return table->switches[table->num_of_switches - 1].switch_rate;
  errorout:
  	return 0;
  }
  
  int dfs_enable_switch(struct dfs_table *table)
  {
  	if (is_gate(table->switch_src_gate))
  		if (pwrcal_gate_enable(table->switch_src_gate))
  			return -1;
  
  	if (is_mux(table->switch_src_usermux))
  		if (pwrcal_mux_set_src(table->switch_src_usermux, 1))
  			return -1;
  
  	return 0;
  }
  
  int dfs_disable_switch(struct dfs_table *table)
  {
  	if (is_mux(table->switch_src_usermux))
  		if (pwrcal_mux_set_src(table->switch_src_usermux, 0))
  			return -1;
  
  	if (is_div(table->switch_src_div))
  		if (pwrcal_div_set_ratio(table->switch_src_div, 1))
  			return -1;
  
  	if (is_gate(table->switch_src_gate))
  		if (pwrcal_gate_disable(table->switch_src_gate))
  			return -1;
  
  	return 0;
  }
  
  int dfs_use_switch(struct dfs_table *table)
  {
  	if (is_mux(table->switch_mux))
  		if (pwrcal_mux_set_src(table->switch_mux,
  					table->switch_use))
  			return -1;
  
  	return 0;
  }
  
  int dfs_not_use_switch(struct dfs_table *table)
  {
  	if (is_mux(table->switch_mux))
  		if (pwrcal_mux_set_src(table->switch_mux, table->switch_notuse))
  			return -1;
  
  	return 0;
  }
  
  int dfs_trans_div(int lv_from, int lv_to, struct dfs_table *table, int opt)
  {
  	unsigned int from;
  	unsigned int to;
  	int trans;
  	int i;
  	struct pwrcal_clk *clk;
  
  	for (i = 1; i < table->num_of_members; i++) {
  		clk = table->members[i];
  		if (is_div(clk)) {
  			if (lv_from >= 0)
  				from = get_value(table, lv_from, i);
  			else
  				from = pwrcal_div_get_ratio(clk) - 1;
  
  			to = get_value(table, lv_to, i);
  
  			trans = 0;
  			switch (opt) {
  			case TRANS_HIGH:
  				if (from < to)
  					trans = 1;
  				break;
  			case TRANS_LOW:
  				if (from > to)
  					trans = 1;
  				break;
  			case TRANS_DIFF:
  				if (from != to)
  					trans = 1;
  				break;
  			case TRANS_FORCE:
  				trans = 1;
  				break;
  			default:
  				break;
  			}
  			if (trans == 0)
  				continue;
  
  			if (pwrcal_div_set_ratio(clk, to + 1))
  				goto errorout;
  		}
  	}
  	return 0;
  
  errorout:
  	pr_err("%s %s %d\n", __func__, clk->name, to + 1);
  	return -1;
  }
  
  int dfs_trans_pll(int lv_from, int lv_to, struct dfs_table *table, int opt)
  {
  	unsigned long long rate;
  	unsigned int from;
  	unsigned int to;
  	int trans;
  	int i;
  	struct pwrcal_clk *clk;
  
  	for (i = 1; i < table->num_of_members; i++) {
  		clk = table->members[i];
  		if (is_pll(clk)) {
  			if (lv_from >= 0) {
  				from = get_value(table, lv_from, i);
  			} else {
  				rate = pwrcal_pll_get_rate(clk);
  				do_div(rate, 1000);
  				from = (unsigned int)rate;
  			}
  
  			to = get_value(table, lv_to, i);
  
  			trans = 0;
  			switch (opt) {
  			case TRANS_HIGH:
  				if (from < to)
  					trans = 1;
  				if (to == 0)
  					trans = 0;
  				break;
  			case TRANS_LOW:
  				if (from > to)
  					trans = 1;
  				if (from == 0)
  					trans = 0;
  				break;
  			case TRANS_DIFF:
  				if (from != to)
  					trans = 1;
  				break;
  			case TRANS_FORCE:
  				trans = 1;
  				break;
  			default:
  				break;
  			}
  
  			if (trans == 0)
  				continue;
  
  			rate = (unsigned long long)to * 1000;
  			if (rate != 0) {
  				if (pwrcal_pll_set_rate(clk, rate))
  					goto errorout;
  				if (pwrcal_pll_is_enabled(clk) != 1)
  					if (pwrcal_pll_enable(clk))
  						goto errorout;
  			} else {
  				if (pwrcal_pll_is_enabled(clk) != 0)
  					if (pwrcal_pll_disable(clk))
  						goto errorout;
  			}
  		}
  	}
  	return 0;
  
  errorout:
  	pr_err("%s %s %d\n", __func__, clk->name, to);
  	return -1;
  }
  
  int dfs_trans_mux(int lv_from, int lv_to, struct dfs_table *table, int opt)
  {
  	unsigned int from;
  	unsigned int to;
  	int trans;
  	int i;
  	struct pwrcal_clk *clk;
  
  	for (i = 1; i < table->num_of_members; i++) {
  		clk = table->members[i];
  		if (is_mux(clk)) {
  			if (lv_from >= 0)
  				from = get_value(table, lv_from, i);
  			else
  				from = pwrcal_mux_get_src(clk);
  
  			to = get_value(table, lv_to, i);
  
  			trans = 0;
  			switch (opt) {
  			case TRANS_HIGH:
  				if (from < to)
  					trans = 1;
  				break;
  			case TRANS_LOW:
  				if (from > to)
  					trans = 1;
  				break;
  			case TRANS_DIFF:
  				if (from != to)
  					trans = 1;
  				break;
  			case TRANS_FORCE:
  				trans = 1;
  				break;
  			default:
  				break;
  			}
  
  			if (trans == 0)
  				continue;
  
  			if (pwrcal_mux_set_src(clk, to) != 0)
  				goto errorout;
  		}
  	}
  
  	return 0;
  
  errorout:
  	pr_err("%s %s %d\n", __func__, clk->name, to);
  	return -1;
  }
  
  int dfs_trans_gate(int lv_from, int lv_to, struct dfs_table *table, int opt)
  {
  	unsigned int from;
  	unsigned int to;
  	int trans;
  	int i;
  	struct pwrcal_clk *clk;
  
  	for (i = 1; i < table->num_of_members; i++) {
  		clk = table->members[i];
  		if (is_gate(clk)) {
  			if (lv_from >= 0)
  				from = get_value(table, lv_from, i);
  			else
  				from = pwrcal_gate_is_enabled(clk);
  
  			to = get_value(table, lv_to, i);
  
  			trans = 0;
  			switch (opt) {
  			case TRANS_HIGH:
  				if (from < to)
  					trans = 1;
  				break;
  			case TRANS_LOW:
  				if (from > to)
  					trans = 1;
  				break;
  			case TRANS_DIFF:
  				if (from != to)
  					trans = 1;
  				break;
  			default:
  				break;
  			}
  
  			if (trans == 0)
  				continue;
  
  			if (to)
  				pwrcal_gate_enable(clk);
  			else
  				pwrcal_gate_disable(clk);
  		}
  	}
  	return 0;
  }
  
  int dfs_get_lv(unsigned int rate, struct dfs_table *table)
  {
  	int i;
  	unsigned int rep;
  
  	if (rate == 0)
  		return -1;
  
  	for (i = 0; i < table->num_of_lv; i++) {
  		rep = *(table->rate_table + (table->num_of_members * i));
  		if (0 != rep && rate >= rep)
  			return i;
  	}
  
  	return i;
  }
  
  
  
  
  static int transition(unsigned int rate_from,
  			unsigned int rate_to,
  			struct dfs_table *table)
  {
  	int lv_from, lv_to, lv_switch;
  	unsigned int rate_switch;
  
  	lv_from = dfs_get_lv(rate_from, table);
  	lv_to = dfs_get_lv(rate_to, table);
  
  	if (lv_from == lv_to)
  		return 0;
  
  	if (lv_from >= table->num_of_lv || lv_to >= table->num_of_lv)
  		goto errorout;
  
  	if (table->trans_pre)
  		table->trans_pre(rate_from, rate_to);
  
  	if (table->num_of_switches != 0) {
  		rate_switch = dfs_set_rate_switch(rate_from, rate_to, table);
  		lv_switch = dfs_get_lv(rate_switch, table);
  
  		if (dfs_enable_switch(table))
  			goto errorout;
  		if (dfs_trans_div(lv_from, lv_switch, table, TRANS_HIGH))
  			goto errorout;
  		if (table->switch_pre)
  			table->switch_pre(rate_from, rate_switch);
  		if (dfs_use_switch(table))
  			goto errorout;
  		if (table->switch_post)
  			table->switch_post(rate_from, rate_switch);
  		if (dfs_trans_mux(lv_from, lv_switch, table, TRANS_DIFF))
  			goto errorout;
  		if (dfs_trans_div(lv_from, lv_switch, table, TRANS_LOW))
  			goto errorout;
  		if (dfs_trans_pll(lv_from, lv_to, table, TRANS_DIFF))
  			goto errorout;
  		if (dfs_trans_div(lv_switch, lv_to, table, TRANS_HIGH))
  			goto errorout;
  		if (table->switch_pre)
  			table->switch_pre(rate_switch, rate_to);
  		if (dfs_not_use_switch(table))
  			goto errorout;
  		if (table->switch_post)
  			table->switch_post(rate_switch, rate_to);
  		if (dfs_trans_mux(lv_switch, lv_to, table, TRANS_DIFF))
  			goto errorout;
  		if (dfs_trans_div(lv_switch, lv_to, table, TRANS_LOW))
  			goto errorout;
  		if (dfs_disable_switch(table))
  			goto errorout;
  	} else {
  		if (dfs_trans_gate(lv_from, lv_to, table, TRANS_HIGH))
  			goto errorout;
  		if (dfs_trans_div(lv_from, lv_to, table, TRANS_HIGH))
  			goto errorout;
  		if (dfs_trans_pll(lv_from, lv_to, table, TRANS_LOW))
  			goto errorout;
  		if (dfs_trans_mux(lv_from, lv_to, table, TRANS_DIFF))
  			goto errorout;
  		if (dfs_trans_pll(lv_from, lv_to, table, TRANS_HIGH))
  			goto errorout;
  		if (dfs_trans_div(lv_from, lv_to, table, TRANS_LOW))
  			goto errorout;
  		if (dfs_trans_gate(lv_from, lv_to, table, TRANS_LOW))
  			goto errorout;
  	}
  
  	if (table->trans_post)
  		table->trans_post(rate_from, rate_to);
  
  	return 0;
  
  errorout:
  	return -1;
  }
  
  static unsigned int get_rate(struct dfs_table *table)
  {
  	int l, m;
  	unsigned int cur[128] = {0, };
  	unsigned long long rate;
  	struct pwrcal_clk *clk;
  
  	for (m = 1; m < table->num_of_members; m++) {
  		clk = table->members[m];
  		if (is_pll(clk)) {
  			rate = pwrcal_pll_get_rate(clk);
  			do_div(rate, 1000);
  			cur[m] = (unsigned int)rate;
  		}
  		if (is_mux(clk))
  			cur[m] = pwrcal_mux_get_src(clk);
  		if (is_div(clk))
  			cur[m] = pwrcal_div_get_ratio(clk) - 1;
  		if (is_gate(clk))
  			cur[m] = pwrcal_gate_is_enabled(clk);
  	}
  
  	for (l = 0; l < table->num_of_lv; l++) {
  		for (m = 1; m < table->num_of_members; m++)
  			if (cur[m] != get_value(table, l, m))
  				break;
  
  		if (m == table->num_of_members)
  			return get_value(table, l, 0);
  	}
  
  	if (is_pll(table->members[1])) {
  		for (l = 0; l < table->num_of_lv; l++)
  			if (cur[1] == get_value(table, l, 1))
  				return get_value(table, l, 0);
  	}
  
  
  	for (m = 1; m < table->num_of_members; m++) {
  		clk = table->members[m];
  		pr_err("dfs_get_rate mid : %s : %d\n", clk->name, cur[m]);
  	}
  
  	return 0;
  }
  
  
  int dfs_get_rate_table(struct dfs_table *dfs, unsigned long *table)
  {
  	int i;
  
  	for (i = 0; i < dfs->num_of_lv; i++)
  		table[i] = get_value(dfs, i, 0);
  
  	return dfs->num_of_lv;
  }
  
  
  int dfs_get_target_rate_table(struct dfs_table *dfs,
  				unsigned int mux,
  				unsigned int div,
  				unsigned long *table)
  {
  	int m, d, i;
  	int num_of_parent;
  	struct pwrcal_clk *parents[32] = {NULL, };
  	unsigned int parents_rate[32] = {0, };
  	unsigned long long rate;
  	unsigned int src, ratio;
  
  	for (m = 0; m < dfs->num_of_members; m++)
  		if (dfs->members[m]->id == mux)
  			break;
  
  	for (d = 0; d < dfs->num_of_members; d++)
  		if (dfs->members[d]->id == div)
  			break;
  
  
  	if (m != dfs->num_of_members) {
  		num_of_parent = pwrcal_mux_get_parents(dfs->members[m],
  								parents);
  		for (i = 0; i < num_of_parent; i++) {
  			rate = pwrcal_clk_get_rate(parents[i]);
  			do_div(rate, 1000);
  			parents_rate[i] = (unsigned int)rate;
  		}
  	} else {
  		parents[0] = pwrcal_clk_get_parent(dfs->members[d]);
  		rate = pwrcal_clk_get_rate(parents[0]);
  		do_div(rate, 1000);
  		parents_rate[0] = (unsigned int)rate;
  	}
  
  
  
  	for (i = 0; i < dfs->num_of_lv; i++) {
  		src = get_value(dfs, i, m);
  		ratio = get_value(dfs, i, d) + 1;
  
  		if (m == dfs->num_of_members) {
  			table[i] = parents_rate[0] / ratio;
  			continue;
  		}
  		if (d == dfs->num_of_members) {
  			table[i] = parents_rate[src];
  			continue;
  		}
  		table[i] = parents_rate[src] / ratio;
  	}
  
  	return i;
  }
  
  
  int dfs_set_rate(struct vclk *vclk, unsigned long to)
  {
  	struct pwrcal_vclk_dfs *dfs;
  	unsigned long ret = -1;
  
  	dfs = to_dfs(vclk);
  
  /*	pr_info("(%s) set from (%ldHz) to (%ldHz) start\n",
  				vclk->name, vclk->vfreq, to);	*/
  	if (transition((unsigned int)(vclk->vfreq),
  			(unsigned int)to,
  			dfs->table))
  		goto out;
  /*	pr_info("(%s) set from (%ldHz) to (%ldHz) success\n",
  				vclk->name, vclk->vfreq, to);	*/
  
  	ret = 0;
  out:
  	if (ret)
  		pr_err("dfs_set_rate error (%s) from(%ld) to(%ld)\n",
  				vclk->name, vclk->vfreq, to);
  
  	return ret;
  }
  
  
  unsigned long dfs_get_rate(struct vclk *vclk)
  {
  	struct pwrcal_vclk_dfs *dfs;
  	unsigned long ret = 0;
  
  	dfs = to_dfs(vclk);
  	ret =  (unsigned long)get_rate(dfs->table);
  
  	return ret;
  }
  
  int dfs_enable(struct vclk *vclk)
  {
  	struct pwrcal_vclk_dfs *dfs;
  	int ret = -1;
  
  	dfs = to_dfs(vclk);
  
  	if (dfs->en_clks)
  		if (set_config(dfs->en_clks, 0))
  			goto out;
  
  	ret = 0;
  out:
  	if (ret)
  		pr_err("p1_enable error (%s)\n", vclk->name);
  
  	return ret;
  }
  
  int dfs_disable(struct vclk *vclk)
  {
  	struct pwrcal_vclk_dfs *dfs;
  	int ret = -1;
  
  	dfs = to_dfs(vclk);
  
  	if (dfs->en_clks != 0)
  		if (set_config(dfs->en_clks, 1))
  			goto out;
  
  	ret = 0;
  out:
  	if (ret)
  		pr_err("dfs_disable error (%s)\n", vclk->name);
  
  	return ret;
  }
  
  int dfs_is_enabled(struct vclk *vclk)
  {
  	if (vclk->ref_count)
  		return 1;
  
  	return 0;
  }
  
  struct vclk_ops dfs_ops = {
  	.enable = dfs_enable,
  	.disable = dfs_disable,
  	.is_enabled = dfs_is_enabled,
  	.get_rate = dfs_get_rate,
  	.set_rate = dfs_set_rate,
  };
  
  unsigned long dfs_get_max_freq(struct vclk *vclk)
  {
  	struct pwrcal_vclk_dfs *dfs = to_dfs(vclk);
  
  	return dfs->table->max_freq;
  }
  
  unsigned long dfs_get_min_freq(struct vclk *vclk)
  {
  	struct pwrcal_vclk_dfs *dfs = to_dfs(vclk);
  
  	return dfs->table->min_freq;
  }
  