static void __init init_cpu_0_dvfs(struct dvfs *cpud)
{
	int i;
	struct dvfs *d = NULL;

	/* Init single G CPU core 0 dvfs if this particular SKU/bin has it.
	   Max rates in multi-core and single-core tables must be the same */
	for (i = 0; i <  ARRAY_SIZE(cpu_0_dvfs_table); i++) {
		if (match_dvfs_one(&cpu_0_dvfs_table[i],
				   cpud->speedo_id, cpud->process_id)) {
			d = &cpu_0_dvfs_table[i];
			break;
		}
	}

	if (d) {
		for (i = 0; i < cpud->num_freqs; i++) {
			d->freqs[i] *= d->freqs_mult;
			if (d->freqs[i] == 0) {
				BUG_ON(i == 0);
				d->freqs[i] = d->freqs[i - 1];
			}
		}
		BUG_ON(cpud->freqs[cpud->num_freqs - 1] !=
		       d->freqs[cpud->num_freqs - 1]);
		cpu_0_freqs = d->freqs;
	}
}
static int __init get_cpu_nominal_mv_index(
	int speedo_id, int process_id, struct dvfs **cpu_dvfs)
{
	int i, j, mv;
	struct dvfs *d;
	struct clk *c;

	/*
	 * Start with nominal level for the chips with this speedo_id. Then,
	 * make sure cpu nominal voltage is below core ("solve from cpu to
	 * core at nominal").
	 */
	BUG_ON(speedo_id >= ARRAY_SIZE(cpu_speedo_nominal_millivolts));
	mv = cpu_speedo_nominal_millivolts[speedo_id];
	if (tegra3_dvfs_rail_vdd_core.nominal_millivolts) {
		int core_mv = tegra3_dvfs_rail_vdd_core.nominal_millivolts;
		while ((mv > tegra3_dvfs_rail_vdd_cpu.min_millivolts) &&
		       (tegra3_get_core_floor_mv(mv) > core_mv))
			mv -= 25;
	}

	/*
	 * Find matching cpu dvfs entry, and use it to determine index to the
	 * final nominal voltage, that satisfies the following requirements:
	 * - allows CPU to run at minimum of the maximum rates specified in
	 *   the dvfs entry and clock tree
	 * - does not violate cpu_to_core dependency as determined above
	 */
	for (i = 0, j = 0; j <  ARRAY_SIZE(cpu_dvfs_table); j++) {
		d = &cpu_dvfs_table[j];
		if (match_dvfs_one(d, speedo_id, process_id)) {
			c = tegra_get_clock_by_name(d->clk_name);
			BUG_ON(!c);

			for (; i < MAX_DVFS_FREQS; i++) {
				if ((d->freqs[i] == 0) ||
				    (cpu_millivolts[i] == 0) ||
				    (mv < cpu_millivolts[i]))
					break;

				if (c->max_rate <= d->freqs[i]*d->freqs_mult) {
					i++;
					break;
				}
			}
			break;
		}
	}

	BUG_ON(i == 0);
	if (j == (ARRAY_SIZE(cpu_dvfs_table) - 1))
		pr_err("tegra3_dvfs: WARNING!!!\n"
		       "tegra3_dvfs: no cpu dvfs table found for chip speedo_id"
		       " %d and process_id %d: set CPU rate limit at %lu\n"
		       "tegra3_dvfs: WARNING!!!\n",
		       speedo_id, process_id, d->freqs[i-1] * d->freqs_mult);

	*cpu_dvfs = d;
	return (i - 1);
}
void __init tegra_soc_init_dvfs(void)
{
	int cpu_speedo_id = tegra_cpu_speedo_id();
	int soc_speedo_id = tegra_soc_speedo_id();
	int cpu_process_id = tegra_cpu_process_id();
#ifdef CONFIG_TEGRA3_LP_CORE_OVERDRIVE
	int core_process_id = 2;
#else
	int core_process_id = tegra_core_process_id();
#endif

	int i;
	int core_nominal_mv_index;
	int cpu_nominal_mv_index;

#ifndef CONFIG_TEGRA_CORE_DVFS
	tegra_dvfs_core_disabled = true;
#endif
#ifndef CONFIG_TEGRA_CPU_DVFS
	tegra_dvfs_cpu_disabled = true;
#endif

	/*
	 * Find nominal voltages for core (1st) and cpu rails before rail
	 * init. Nominal voltage index in the scaling ladder will also be
	 * used to determine max dvfs frequency for the respective domains.
	 */
	core_nominal_mv_index = get_core_nominal_mv_index(soc_speedo_id);
	if (core_nominal_mv_index < 0) {
		tegra3_dvfs_rail_vdd_core.disabled = true;
		tegra_dvfs_core_disabled = true;
		core_nominal_mv_index = 0;
	}
	tegra3_dvfs_rail_vdd_core.nominal_millivolts =
		core_millivolts[core_nominal_mv_index];

	cpu_nominal_mv_index = get_cpu_nominal_mv_index(
		cpu_speedo_id, cpu_process_id, &cpu_dvfs);
	BUG_ON((cpu_nominal_mv_index < 0) || (!cpu_dvfs));
	tegra3_dvfs_rail_vdd_cpu.nominal_millivolts =
		cpu_millivolts[cpu_nominal_mv_index];

	/* Init rail structures and dependencies */
	tegra_dvfs_init_rails(tegra3_dvfs_rails, ARRAY_SIZE(tegra3_dvfs_rails));
	tegra_dvfs_add_relationships(tegra3_dvfs_relationships,
		ARRAY_SIZE(tegra3_dvfs_relationships));

	/* Search core dvfs table for speedo/process matching entries and
	   initialize dvfs-ed clocks */
	for (i = 0; i <  ARRAY_SIZE(core_dvfs_table); i++) {
		struct dvfs *d = &core_dvfs_table[i];
		if (!match_dvfs_one(d, soc_speedo_id, core_process_id))
			continue;
		init_dvfs_one(d, core_nominal_mv_index);
	}

	/* Initialize matching cpu dvfs entry already found when nominal
	   voltage was determined */
	init_dvfs_one(cpu_dvfs, cpu_nominal_mv_index);
	init_dvfs_cold(cpu_dvfs, cpu_nominal_mv_index);

	/* Finally disable dvfs on rails if necessary */
	if (tegra_dvfs_core_disabled)
		tegra_dvfs_rail_disable(&tegra3_dvfs_rail_vdd_core);
	if (tegra_dvfs_cpu_disabled)
		tegra_dvfs_rail_disable(&tegra3_dvfs_rail_vdd_cpu);

	pr_info("tegra dvfs: VDD_CPU nominal %dmV, scaling %s\n",
		tegra3_dvfs_rail_vdd_cpu.nominal_millivolts,
		tegra_dvfs_cpu_disabled ? "disabled" : "enabled");
	pr_info("tegra dvfs: VDD_CORE nominal %dmV, scaling %s\n",
		tegra3_dvfs_rail_vdd_core.nominal_millivolts,
		tegra_dvfs_core_disabled ? "disabled" : "enabled");
}
static int __init get_cpu_nominal_mv_index(
	int speedo_id, int process_id, struct dvfs **cpu_dvfs)
{
	int i, j, mv, nom_index;
	struct dvfs *d;
	struct clk *c;

	/*
	 * Find maximum cpu voltage that satisfies cpu_to_core dependency for
	 * nominal core voltage ("solve from cpu to core at nominal"). Clip
	 * result to the nominal cpu level for the chips with this speedo_id.
	 */
	mv = tegra3_dvfs_rail_vdd_core.nominal_millivolts;
	for (i = 0; i < MAX_DVFS_FREQS; i++) {
		if ((cpu_millivolts[i] == 0) ||
		    tegra3_get_core_floor_mv(cpu_millivolts[i]) > mv)
			break;
	}
	BUG_ON(i == 0);
	mv = cpu_millivolts[i - 1];
	pr_info("cpu_nominal_mv: %i\n", mv);
	BUG_ON(mv < tegra3_dvfs_rail_vdd_cpu.min_millivolts);
	mv = min(mv, tegra_cpu_speedo_mv());
	pr_info("cpu_nominal_mv_min: %i\n", mv);

	/*
	 * Find matching cpu dvfs entry, and use it to determine index to the
	 * final nominal voltage, that satisfies the following requirements:
	 * - allows CPU to run at minimum of the maximum rates specified in
	 *   the dvfs entry and clock tree
	 * - does not violate cpu_to_core dependency as determined above
	 */
	for (i = 0, j = 0; j <  ARRAY_SIZE(cpu_dvfs_table); j++) {
		d = &cpu_dvfs_table[j];
		if (match_dvfs_one(d, speedo_id, process_id)) {
			c = tegra_get_clock_by_name(d->clk_name);
			BUG_ON(!c);

			for (; i < MAX_DVFS_FREQS; i++) {
				if ((d->freqs[i] == 0) ||
				    (cpu_millivolts[i] == 0) ||
				    (mv < cpu_millivolts[i]))
					break;

				if (c->max_rate <= d->freqs[i]*d->freqs_mult) {
					i++;
					break;
				}
			}
			break;
		}
	}

	pr_info("dvfs: freqs_mult: %i\n", d->freqs_mult);

	BUG_ON(i == 0);
	if (j == (ARRAY_SIZE(cpu_dvfs_table) - 1))
		pr_err("tegra3_dvfs: WARNING!!!\n"
		       "tegra3_dvfs: no cpu dvfs table found for chip speedo_id"
		       " %d and process_id %d: set CPU rate limit at %lu\n"
		       "tegra3_dvfs: WARNING!!!\n",
		       speedo_id, process_id, d->freqs[i-1] * d->freqs_mult);

	*cpu_dvfs = d;

	nom_index = i - 1;
	pr_info("cpu_nominal_mv_index: %i\n", nom_index);

	pr_info("cpu_dvfs->speedo_id: %i\n", d->speedo_id);
	pr_info("cpu_dvfs->process_id: %i\n", d->process_id);
	for (i=0;i<MAX_DVFS_FREQS;i++) {
		pr_info("cpu_dvfs->freqs: %lu\n", d->freqs[i]);
	}

	return nom_index;
}