262 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2// Copyright (C) 2014 Broadcom Corporation3 4#include <linux/kernel.h>5#include <linux/err.h>6#include <linux/clk-provider.h>7#include <linux/io.h>8#include <linux/of.h>9#include <linux/clkdev.h>10#include <linux/of_address.h>11#include <linux/delay.h>12 13#include "clk-iproc.h"14 15struct iproc_asiu;16 17struct iproc_asiu_clk {18 struct clk_hw hw;19 const char *name;20 struct iproc_asiu *asiu;21 unsigned long rate;22 struct iproc_asiu_div div;23 struct iproc_asiu_gate gate;24};25 26struct iproc_asiu {27 void __iomem *div_base;28 void __iomem *gate_base;29 30 struct clk_hw_onecell_data *clk_data;31 struct iproc_asiu_clk *clks;32};33 34#define to_asiu_clk(hw) container_of(hw, struct iproc_asiu_clk, hw)35 36static int iproc_asiu_clk_enable(struct clk_hw *hw)37{38 struct iproc_asiu_clk *clk = to_asiu_clk(hw);39 struct iproc_asiu *asiu = clk->asiu;40 u32 val;41 42 /* some clocks at the ASIU level are always enabled */43 if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET)44 return 0;45 46 val = readl(asiu->gate_base + clk->gate.offset);47 val |= (1 << clk->gate.en_shift);48 writel(val, asiu->gate_base + clk->gate.offset);49 50 return 0;51}52 53static void iproc_asiu_clk_disable(struct clk_hw *hw)54{55 struct iproc_asiu_clk *clk = to_asiu_clk(hw);56 struct iproc_asiu *asiu = clk->asiu;57 u32 val;58 59 /* some clocks at the ASIU level are always enabled */60 if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET)61 return;62 63 val = readl(asiu->gate_base + clk->gate.offset);64 val &= ~(1 << clk->gate.en_shift);65 writel(val, asiu->gate_base + clk->gate.offset);66}67 68static unsigned long iproc_asiu_clk_recalc_rate(struct clk_hw *hw,69 unsigned long parent_rate)70{71 struct iproc_asiu_clk *clk = to_asiu_clk(hw);72 struct iproc_asiu *asiu = clk->asiu;73 u32 val;74 unsigned int div_h, div_l;75 76 if (parent_rate == 0) {77 clk->rate = 0;78 return 0;79 }80 81 /* if clock divisor is not enabled, simply return parent rate */82 val = readl(asiu->div_base + clk->div.offset);83 if ((val & (1 << clk->div.en_shift)) == 0) {84 clk->rate = parent_rate;85 return parent_rate;86 }87 88 /* clock rate = parent rate / (high_div + 1) + (low_div + 1) */89 div_h = (val >> clk->div.high_shift) & bit_mask(clk->div.high_width);90 div_h++;91 div_l = (val >> clk->div.low_shift) & bit_mask(clk->div.low_width);92 div_l++;93 94 clk->rate = parent_rate / (div_h + div_l);95 pr_debug("%s: rate: %lu. parent rate: %lu div_h: %u div_l: %u\n",96 __func__, clk->rate, parent_rate, div_h, div_l);97 98 return clk->rate;99}100 101static long iproc_asiu_clk_round_rate(struct clk_hw *hw, unsigned long rate,102 unsigned long *parent_rate)103{104 unsigned int div;105 106 if (rate == 0 || *parent_rate == 0)107 return -EINVAL;108 109 if (rate == *parent_rate)110 return *parent_rate;111 112 div = DIV_ROUND_CLOSEST(*parent_rate, rate);113 if (div < 2)114 return *parent_rate;115 116 return *parent_rate / div;117}118 119static int iproc_asiu_clk_set_rate(struct clk_hw *hw, unsigned long rate,120 unsigned long parent_rate)121{122 struct iproc_asiu_clk *clk = to_asiu_clk(hw);123 struct iproc_asiu *asiu = clk->asiu;124 unsigned int div, div_h, div_l;125 u32 val;126 127 if (rate == 0 || parent_rate == 0)128 return -EINVAL;129 130 /* simply disable the divisor if one wants the same rate as parent */131 if (rate == parent_rate) {132 val = readl(asiu->div_base + clk->div.offset);133 val &= ~(1 << clk->div.en_shift);134 writel(val, asiu->div_base + clk->div.offset);135 return 0;136 }137 138 div = DIV_ROUND_CLOSEST(parent_rate, rate);139 if (div < 2)140 return -EINVAL;141 142 div_h = div_l = div >> 1;143 div_h--;144 div_l--;145 146 val = readl(asiu->div_base + clk->div.offset);147 val |= 1 << clk->div.en_shift;148 if (div_h) {149 val &= ~(bit_mask(clk->div.high_width)150 << clk->div.high_shift);151 val |= div_h << clk->div.high_shift;152 } else {153 val &= ~(bit_mask(clk->div.high_width)154 << clk->div.high_shift);155 }156 if (div_l) {157 val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift);158 val |= div_l << clk->div.low_shift;159 } else {160 val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift);161 }162 writel(val, asiu->div_base + clk->div.offset);163 164 return 0;165}166 167static const struct clk_ops iproc_asiu_ops = {168 .enable = iproc_asiu_clk_enable,169 .disable = iproc_asiu_clk_disable,170 .recalc_rate = iproc_asiu_clk_recalc_rate,171 .round_rate = iproc_asiu_clk_round_rate,172 .set_rate = iproc_asiu_clk_set_rate,173};174 175void __init iproc_asiu_setup(struct device_node *node,176 const struct iproc_asiu_div *div,177 const struct iproc_asiu_gate *gate,178 unsigned int num_clks)179{180 int i, ret;181 struct iproc_asiu *asiu;182 183 if (WARN_ON(!gate || !div))184 return;185 186 asiu = kzalloc(sizeof(*asiu), GFP_KERNEL);187 if (WARN_ON(!asiu))188 return;189 190 asiu->clk_data = kzalloc(struct_size(asiu->clk_data, hws, num_clks),191 GFP_KERNEL);192 if (WARN_ON(!asiu->clk_data))193 goto err_clks;194 asiu->clk_data->num = num_clks;195 196 asiu->clks = kcalloc(num_clks, sizeof(*asiu->clks), GFP_KERNEL);197 if (WARN_ON(!asiu->clks))198 goto err_asiu_clks;199 200 asiu->div_base = of_iomap(node, 0);201 if (WARN_ON(!asiu->div_base))202 goto err_iomap_div;203 204 asiu->gate_base = of_iomap(node, 1);205 if (WARN_ON(!asiu->gate_base))206 goto err_iomap_gate;207 208 for (i = 0; i < num_clks; i++) {209 struct clk_init_data init;210 const char *parent_name;211 struct iproc_asiu_clk *asiu_clk;212 const char *clk_name;213 214 ret = of_property_read_string_index(node, "clock-output-names",215 i, &clk_name);216 if (WARN_ON(ret))217 goto err_clk_register;218 219 asiu_clk = &asiu->clks[i];220 asiu_clk->name = clk_name;221 asiu_clk->asiu = asiu;222 asiu_clk->div = div[i];223 asiu_clk->gate = gate[i];224 init.name = clk_name;225 init.ops = &iproc_asiu_ops;226 init.flags = 0;227 parent_name = of_clk_get_parent_name(node, 0);228 init.parent_names = (parent_name ? &parent_name : NULL);229 init.num_parents = (parent_name ? 1 : 0);230 asiu_clk->hw.init = &init;231 232 ret = clk_hw_register(NULL, &asiu_clk->hw);233 if (WARN_ON(ret))234 goto err_clk_register;235 asiu->clk_data->hws[i] = &asiu_clk->hw;236 }237 238 ret = of_clk_add_hw_provider(node, of_clk_hw_onecell_get,239 asiu->clk_data);240 if (WARN_ON(ret))241 goto err_clk_register;242 243 return;244 245err_clk_register:246 while (--i >= 0)247 clk_hw_unregister(asiu->clk_data->hws[i]);248 iounmap(asiu->gate_base);249 250err_iomap_gate:251 iounmap(asiu->div_base);252 253err_iomap_div:254 kfree(asiu->clks);255 256err_asiu_clks:257 kfree(asiu->clk_data);258 259err_clks:260 kfree(asiu);261}262