#include <linux/clk.h>
#include <linux/clk-provider.h>
#include <linux/clk/clk-conf.h>
#include <linux/device.h>
#include <linux/of.h>
#include <linux/printk.h>
#include <linux/slab.h>
static int __set_clk_parents(struct device_node *node, bool clk_supplier)
{
struct of_phandle_args clkspec;
int index, rc, num_parents;
struct clk *clk, *pclk;
num_parents = of_count_phandle_with_args(node, "assigned-clock-parents",
"#clock-cells");
if (num_parents == -EINVAL)
pr_err("clk: invalid value of clock-parents property at %pOF\n",
node);
for (index = 0; index < num_parents; index++) {
rc = of_parse_phandle_with_args_map(node, "assigned-clock-parents",
"clock", index, &clkspec);
if (rc < 0) {
if (rc == -ENOENT)
continue;
else
return rc;
}
if (clkspec.np == node && !clk_supplier) {
of_node_put(clkspec.np);
return 0;
}
pclk = of_clk_get_from_provider(&clkspec);
of_node_put(clkspec.np);
if (IS_ERR(pclk)) {
if (PTR_ERR(pclk) != -EPROBE_DEFER)
pr_warn("clk: couldn't get parent clock %d for %pOF\n",
index, node);
return PTR_ERR(pclk);
}
rc = of_parse_phandle_with_args_map(node, "assigned-clocks",
"clock", index, &clkspec);
if (rc < 0)
goto err;
if (clkspec.np == node && !clk_supplier) {
of_node_put(clkspec.np);
rc = 0;
goto err;
}
clk = of_clk_get_from_provider(&clkspec);
of_node_put(clkspec.np);
if (IS_ERR(clk)) {
if (PTR_ERR(clk) != -EPROBE_DEFER)
pr_warn("clk: couldn't get assigned clock %d for %pOF\n",
index, node);
rc = PTR_ERR(clk);
goto err;
}
rc = clk_set_parent(clk, pclk);
if (rc < 0)
pr_err("clk: failed to reparent %s to %s: %d\n",
__clk_get_name(clk), __clk_get_name(pclk), rc);
clk_put(clk);
clk_put(pclk);
}
return 0;
err:
clk_put(pclk);
return rc;
}
static int __set_clk_rates(struct device_node *node, bool clk_supplier)
{
struct of_phandle_args clkspec;
int rc, count, count_64, index;
struct clk *clk;
u64 *rates_64 __free(kfree) = NULL;
u32 *rates __free(kfree) = NULL;
count = of_property_count_u32_elems(node, "assigned-clock-rates");
count_64 = of_property_count_u64_elems(node, "assigned-clock-rates-u64");
if (count_64 > 0) {
count = count_64;
rates_64 = kzalloc_objs(*rates_64, count);
if (!rates_64)
return -ENOMEM;
rc = of_property_read_u64_array(node,
"assigned-clock-rates-u64",
rates_64, count);
} else if (count > 0) {
rates = kzalloc_objs(*rates, count);
if (!rates)
return -ENOMEM;
rc = of_property_read_u32_array(node, "assigned-clock-rates",
rates, count);
} else {
return 0;
}
if (rc)
return rc;
for (index = 0; index < count; index++) {
unsigned long rate;
if (rates_64)
rate = rates_64[index];
else
rate = rates[index];
if (rate) {
rc = of_parse_phandle_with_args_map(node, "assigned-clocks",
"clock", index, &clkspec);
if (rc < 0) {
if (rc == -ENOENT)
continue;
else
return rc;
}
if (clkspec.np == node && !clk_supplier) {
of_node_put(clkspec.np);
return 0;
}
clk = of_clk_get_from_provider(&clkspec);
of_node_put(clkspec.np);
if (IS_ERR(clk)) {
if (PTR_ERR(clk) != -EPROBE_DEFER)
pr_warn("clk: couldn't get clock %d for %pOF\n",
index, node);
return PTR_ERR(clk);
}
rc = clk_set_rate(clk, rate);
if (rc < 0)
pr_err("clk: couldn't set %s clk rate to %lu (%d), current rate: %lu\n",
__clk_get_name(clk), rate, rc,
clk_get_rate(clk));
clk_put(clk);
}
}
return 0;
}
static int __set_clk_spread_spectrum(struct device_node *node, bool clk_supplier)
{
u32 elem_size = sizeof(struct clk_spread_spectrum);
struct clk_spread_spectrum *sscs;
struct of_phandle_args clkspec;
int rc, count, index;
struct clk *clk;
count = of_property_count_elems_of_size(node, "assigned-clock-sscs", elem_size);
if (count <= 0)
return 0;
sscs = kcalloc(count, elem_size, GFP_KERNEL);
if (!sscs)
return -ENOMEM;
rc = of_property_read_u32_array(node, "assigned-clock-sscs", (u32 *)sscs,
count * 3);
if (rc)
goto free_sscs;
for (index = 0; index < count; index++) {
struct clk_spread_spectrum *conf = &sscs[index];
struct clk_hw *hw;
if (!conf->modfreq_hz && !conf->spread_bp && !conf->method)
continue;
rc = of_parse_phandle_with_args(node, "assigned-clocks", "#clock-cells",
index, &clkspec);
if (rc < 0) {
if (rc == -ENOENT) {
rc = 0;
continue;
} else
goto free_sscs;
}
if (clkspec.np == node && !clk_supplier) {
of_node_put(clkspec.np);
goto free_sscs;
}
clk = of_clk_get_from_provider(&clkspec);
of_node_put(clkspec.np);
if (IS_ERR(clk)) {
if (PTR_ERR(clk) != -EPROBE_DEFER)
pr_warn("clk: couldn't get clock %d for %pOF\n",
index, node);
rc = PTR_ERR(clk);
goto free_sscs;
}
hw = __clk_get_hw(clk);
rc = clk_hw_set_spread_spectrum(hw, conf);
if (rc < 0) {
pr_err("clk: couldn't set %s clk spread spectrum %u %u %u: %d\n",
__clk_get_name(clk), conf->modfreq_hz, conf->spread_bp,
conf->method, rc);
rc = 0;
}
clk_put(clk);
}
free_sscs:
kfree(sscs);
return rc;
}
int of_clk_set_defaults(struct device_node *node, bool clk_supplier)
{
int rc;
if (!node)
return 0;
rc = __set_clk_spread_spectrum(node, clk_supplier);
if (rc < 0)
return rc;
rc = __set_clk_parents(node, clk_supplier);
if (rc < 0)
return rc;
return __set_clk_rates(node, clk_supplier);
}
EXPORT_SYMBOL_GPL