/*
 * Copyright (C) 2016 Linaro.
 * Viresh Kumar <[email protected]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <linux/cpu.h>
#include <linux/pm_domain.h>
#include <linux/slab.h>

static int pd_power_on(struct generic_pm_domain *domain)
{
	pr_info("%s: %d\n", __func__, __LINE__);
	return 0;
}

static int pd_power_off(struct generic_pm_domain *domain)
{
	pr_info("%s: %d\n", __func__, __LINE__);
	return 0;
}

int pd_get_performance(struct device *dev, unsigned long rate)
{
	struct device *cdev = get_cpu_device(0);
	WARN_ON(!rate);
	WARN_ON(cdev != dev);

	dev_info(dev, "%s: freq: %lu\n", __func__, rate);

	if (rate <= 500000000)
		return 1;
	else if (rate <= 500000000)
		return 2;
	else if (rate <= 900000000)
		return 3;
	else if (rate <= 1200000000)
		return 4;
	else if (rate <= 1600000000)
		return 5;
	else
		return 6;
}

static int pd_set_performance(struct generic_pm_domain *domain, unsigned int state)
{
	pr_info("%s: %d: %d\n", __func__, __LINE__, state);
	return 0;
}

static const struct of_device_id pm_domain_of_match[] __initconst = {
	{
		.compatible = "foo,genpd",
	},
	{ },
};

static int __init add_domains(void)
{
	struct device_node *np;
	struct generic_pm_domain *pd;

	for_each_matching_node_and_match(np, pm_domain_of_match, NULL) {
		pd = kzalloc(sizeof(*pd), GFP_KERNEL);
		if (!pd)
			return -ENOMEM;

		pd->name = kstrdup_const(strrchr(np->full_name, '/') + 1,
				GFP_KERNEL);
		if (!pd->name) {
			of_node_put(np);
			return -1;
		}

		pd->power_off = pd_power_off;
		pd->power_on = pd_power_on;

		if (of_find_property(np, "power-domains", NULL))
			pd->get_performance_state = pd_get_performance;
		else
			pd->set_performance_state = pd_set_performance;

		pm_genpd_init(pd, NULL, false);
		of_genpd_add_provider_simple(np, pd);

		pr_info("%s: %d: %p\n", __func__, __LINE__, pd);
	}

	return 0;
}

static int __init add_subdomains(void)
{
	struct device_node *np;

	for_each_matching_node_and_match(np, pm_domain_of_match, NULL) {
		struct of_phandle_args child, parent1, parent2;

		if (of_parse_phandle_with_args(np, "power-domains",
					       "#power-domain-cells", 0,
					       &parent1) ||
		    of_parse_phandle_with_args(np, "power-domains",
					       "#power-domain-cells", 1,
					       &parent2))
			continue;

		child.np = np;
		child.args_count = 0;

		if (of_genpd_add_subdomain(&parent1, &child) ||
		    of_genpd_add_subdomain(&parent2, &child))
			pr_warn("%s failed to add subdomain: %s\n",
				parent1.np->full_name, child.np->full_name);
		else
			pr_info("%s has as child subdomain: %s.\n",
				parent1.np->full_name, child.np->full_name);
	}

	return 0;
}

static int __init genpd_test_init(void)
{
	struct device *dev = get_cpu_device(0);

	if (add_domains()) {
		pr_info("%s: %d\n", __func__, __LINE__);
		return -ENOMEM;
	}

	if (add_subdomains()) {
		pr_info("%s: %d\n", __func__, __LINE__);
		return -ENOMEM;
	}

	return dev_pm_domain_attach(dev, false);
}
device_initcall(genpd_test_init);