diff -drupN a/drivers/clk/sunxi/clk-cpu.c b/drivers/clk/sunxi/clk-cpu.c --- a/drivers/clk/sunxi/clk-cpu.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/clk/sunxi/clk-cpu.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2019 Allwinnertech. + * Author:huanghuafeng + * + * base on clk/samsung/clk-cpu.c + * 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. + * + * Adjustable factor-based clock implementation + */ +#include +#include +#include +#include +#include +#include +#include "clk-cpu.h" +#include "clk-debugfs.h" + +static int sunxi_cpuclk_pre_rate_change(struct clk_notifier_data *ndata, + struct sunxi_cpuclk *cpuclk) +{ + int ret; + struct clk *clk = cpuclk->clk; + struct clk *parent = cpuclk->alt_parent; + + ret = clk_set_parent(clk, parent); + if (ret) { + pr_err("%s: failed to switch alternate parent %s\n", + __func__, parent->core->name); + return ret; + } + + return ret; +} + +static int sunxi_cpuclk_post_rate_change(struct clk_notifier_data *ndata, + struct sunxi_cpuclk *cpuclk) +{ + int ret; + struct clk *clk = cpuclk->clk; + struct clk *parent = cpuclk->parent; + + ret = clk_set_parent(clk, parent); + if (ret) { + pr_err("%s: failed to switch alternate parent %s\n", + __func__, parent->core->name); + return ret; + } + return ret; + +} + +/* + * This notifier function is called for the pre-rate and post-rate change + * notifications of the parent clock of cpuclk. + */ +static int sunxi_cpuclk_notifier_cb(struct notifier_block *nb, + unsigned long event, void *data) +{ + struct clk_notifier_data *ndata = data; + struct sunxi_cpuclk *cpuclk; + int ret; + + cpuclk = container_of(nb, struct sunxi_cpuclk, clk_nb); + + if (event == PRE_RATE_CHANGE) + ret = sunxi_cpuclk_pre_rate_change(ndata, cpuclk); + else if (event == POST_RATE_CHANGE) + ret = sunxi_cpuclk_post_rate_change(ndata, cpuclk); + + return notifier_from_errno(ret); +} + +struct clk *sunxi_clk_register_cpu(struct periph_init_data *pd, + void __iomem *base, const char *alt_parent, const char *parent) +{ + struct sunxi_cpuclk *cpuclk; + struct clk *clk; + struct clk_init_data init; + int ret; + + BUG_ON((pd == NULL) && (pd->periph == NULL)); + + cpuclk = kzalloc(sizeof(*cpuclk), GFP_KERNEL); + if (!cpuclk) { + pr_err("%s: could not allocate cpuclk\n", __func__); + return ERR_PTR(-ENOMEM); + } + +#ifdef __SUNXI_ALL_CLK_IGNORE_UNUSED__ + pd->flags |= CLK_IGNORE_UNUSED; +#endif + + cpuclk->periph = pd->periph; + init.name = pd->name; + + init.ops = cpuclk->periph->priv_clkops + ? cpuclk->periph->priv_clkops + : (&sunxi_clk_periph_ops); + + init.flags = pd->flags; + init.parent_names = pd->parent_names; + init.num_parents = pd->num_parents; + + /* Data in .init is copied by clk_register(), so stack variable OK */ + cpuclk->periph->hw.init = &init; + cpuclk->periph->flags = init.flags; + + /* fix registers */ + cpuclk->periph->mux.reg = cpuclk->periph->mux.reg ? (base + + (unsigned long __force)cpuclk->periph->mux.reg) : NULL; + + cpuclk->periph->divider.reg = cpuclk->periph->divider.reg ? (base + + (unsigned long __force)cpuclk->periph->divider.reg) : NULL; + + cpuclk->periph->gate.enable = cpuclk->periph->gate.enable ? (base + + (unsigned long __force)cpuclk->periph->gate.enable) : NULL; + + cpuclk->periph->gate.reset = cpuclk->periph->gate.reset ? (base + + (unsigned long __force)cpuclk->periph->gate.reset) : NULL; + + cpuclk->periph->gate.bus = cpuclk->periph->gate.bus ? (base + + (unsigned long __force)cpuclk->periph->gate.bus) : NULL; + + cpuclk->periph->gate.dram = cpuclk->periph->gate.dram ? (base + + (unsigned long __force)cpuclk->periph->gate.dram) : NULL; + + cpuclk->clk_nb.notifier_call = sunxi_cpuclk_notifier_cb; + + cpuclk->alt_parent = __clk_lookup(alt_parent); + if (IS_ERR(cpuclk->alt_parent)) { + pr_err("%s: could not lookup alternate parent %s\n", + __func__, alt_parent); + return cpuclk->alt_parent; + } + + cpuclk->parent = __clk_lookup(parent); + if (IS_ERR(cpuclk->parent)) { + pr_err("%s: could not lookup parent clock %s\n", + __func__, parent); + return cpuclk->parent; + } + + ret = clk_notifier_register(cpuclk->parent, &cpuclk->clk_nb); + if (ret) { + pr_err("%s: failed to register clock notifier for %s\n", + __func__, parent); + return ERR_PTR(ret); + } + + clk = clk_register(NULL, &cpuclk->periph->hw); + if (IS_ERR(clk)) + return clk; + + cpuclk->clk = clk; + return clk; +} + +