#include <linux/bitfield.h>
#include <linux/clk-provider.h>
#include <linux/clk.h>
#include <linux/container_of.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/errno.h>
#include <linux/math.h>
#include <linux/rational.h>
#include <linux/regmap.h>
#include <linux/types.h>
#include <linux/units.h>
#include "clk-zx.h"
#define ZX29_PLL_DISABLE BIT(31)
#define ZX29_PLL_LOCKED BIT(30)
#define ZX29_PLL_LOCK_FILTER BIT(29)
#define ZX29_PLL_DSM_DISABLE BIT(27)
#define ZX29_PLL_PARENT_MASK GENMASK(26, 25)
#define ZX29_PLL_BYPASS BIT(24)
#define ZX29_PLL_REFDIV_MASK GENMASK(23, 18)
#define ZX29_PLL_FBDIV_MASK GENMASK(17, 6)
#define ZX29_PLL_POSTDIV1_MASK GENMASK(5, 3)
#define ZX29_PLL_POSTDIV2_MASK GENMASK(2, 0)
#define ZX29_PLL_REG2_OFFSET 4
#define ZX29_PLL_DACAP BIT(27)
#define ZX29_PLL_4PHASE_OUT_DISABLE BIT(26)
#define ZX29_PLL_POSTDIV_OUT_DISABLE BIT(25)
#define ZX29_PLL_VCO_OUT_DISABLE BIT(24)
#define ZX29_PLL_FRACT GENMASK(23, 0)
#define ZX29_PLL_VCO_MAX_FREQ (1300 * HZ_PER_MHZ)
#define ZX29_PLL_VCO_MIN_FREQ (900 * HZ_PER_MHZ)
struct zx29_clk_pll {
struct clk_hw hw;
struct device *dev;
struct regmap *map;
u16 reg;
};
static inline struct zx29_clk_pll *to_zx29_clk_pll(struct clk_hw *hw)
{
return container_of(hw, struct zx29_clk_pll, hw);
}
static int zx29_pll_is_prepared(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
int res;
res = regmap_test_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
if (res < 0)
return res;
return !res;
}
static int zx29_pll_prepare(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
u32 val = 0;
int res;
res = regmap_clear_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
if (res < 0)
return res;
res = regmap_read_poll_timeout(pll->map, pll->reg, val, val & ZX29_PLL_LOCKED, 50, 2000);
if (res) {
regmap_set_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
dev_err(pll->dev, "%s: PLL prepare failed: %d. Config value 0x%08x\n",
clk_hw_get_name(&pll->hw), res, val);
}
return res;
}
static void zx29_pll_unprepare(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
regmap_set_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
}
static int zx29_pll_is_enabled(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
int res;
res = regmap_test_bits(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET,
ZX29_PLL_POSTDIV_OUT_DISABLE);
if (res < 0)
return res;
return !res;
}
static int zx29_pll_enable(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
return regmap_clear_bits(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET,
ZX29_PLL_POSTDIV_OUT_DISABLE);
}
static void zx29_pll_disable(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
regmap_set_bits(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET,
ZX29_PLL_POSTDIV_OUT_DISABLE);
}
static unsigned long zx29_pll_get_rate(const struct zx29_clk_pll *pll, unsigned long parent_rate,
u32 setting)
{
unsigned long refdiv, fbdiv, postdiv1, postdiv2, freq;
const char *name = clk_hw_get_name(&pll->hw);
u64 vco;
refdiv = FIELD_GET(ZX29_PLL_REFDIV_MASK, setting);
fbdiv = FIELD_GET(ZX29_PLL_FBDIV_MASK, setting);
postdiv1 = FIELD_GET(ZX29_PLL_POSTDIV1_MASK, setting);
postdiv2 = FIELD_GET(ZX29_PLL_POSTDIV2_MASK, setting);
dev_dbg(pll->dev, "%s: reference clock %lu Hz, PLL setting 0x%08x\n",
name, parent_rate, setting);
if (!refdiv || !postdiv1 || !postdiv2) {
dev_err(pll->dev, "%s: divide by zero (%lu, %lu, %lu)\n", name, refdiv, postdiv1,
postdiv2);
return 0;
}
vco = div_u64((u64)parent_rate * fbdiv, refdiv);
freq = div_u64(div_u64(vco, postdiv1), postdiv2);
dev_dbg(pll->dev, "%s: refdiv %lu fbdiv %lu\n", name, refdiv, fbdiv);
dev_dbg(pll->dev, "%s: postdiv1 %lu postdiv2 %lu\n", name, postdiv1, postdiv2);
dev_dbg(pll->dev, "%s: %lu MHz\n", name, freq / HZ_PER_MHZ);
return freq;
}
static unsigned long zx29_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
u32 val;
int res;
res = regmap_read(pll->map, pll->reg, &val);
if (res < 0) {
dev_err(pll->dev, "%s: Failed to read PLL settings\n", clk_hw_get_name(&pll->hw));
return 0;
}
return zx29_pll_get_rate(pll, parent_rate, val);
}
static u32 zx29_pll_calc_values(const struct zx29_clk_pll *pll, unsigned long parent_rate,
unsigned long rate, unsigned long min_rate, unsigned long max_rate)
{
const unsigned int postdiv1_max = (1 << hweight32(ZX29_PLL_POSTDIV1_MASK)) - 1;
const unsigned int postdiv2_max = (1 << hweight32(ZX29_PLL_POSTDIV2_MASK)) - 1;
unsigned long fbdiv, refdiv, best_fbdiv = 0, best_refdiv = 0;
u32 postdiv1 = 0, postdiv2 = 0, i, j, setting;
const char *name = clk_hw_get_name(&pll->hw);
long best = LONG_MAX;
for (j = 1; j <= postdiv2_max; j++) {
for (i = 1; i <= postdiv1_max; i++) {
u64 vco = (u64)rate * i * j;
unsigned long out;
if (vco > ZX29_PLL_VCO_MAX_FREQ || vco < ZX29_PLL_VCO_MIN_FREQ)
continue;
rational_best_approximation(vco, parent_rate,
(1 << hweight32(ZX29_PLL_FBDIV_MASK)) - 1,
(1 << hweight32(ZX29_PLL_REFDIV_MASK)) - 1,
&fbdiv, &refdiv);
setting = FIELD_PREP(ZX29_PLL_FBDIV_MASK, fbdiv);
setting |= FIELD_PREP(ZX29_PLL_REFDIV_MASK, refdiv);
setting |= FIELD_PREP(ZX29_PLL_POSTDIV1_MASK, i);
setting |= FIELD_PREP(ZX29_PLL_POSTDIV2_MASK, j);
out = zx29_pll_get_rate(pll, parent_rate, setting);
if (out < min_rate || out > max_rate)
continue;
if (abs_diff(out, rate) > best)
continue;
if (abs_diff(out, rate) < best) {
postdiv1 = i;
postdiv2 = j;
best_fbdiv = fbdiv;
best_refdiv = refdiv;
best = abs_diff(out, rate);
if (!best)
goto search_done;
}
}
}
search_done:
if (!postdiv1) {
dev_err(pll->dev, "Did not find a setting for %lu Hz, parent %lu Hz\n",
rate, parent_rate);
return 0;
}
dev_dbg(pll->dev, "%s: parent rate %lu\n", name, parent_rate);
dev_dbg(pll->dev, "%s: found VCO dividers %u and %u\n", name, postdiv1, postdiv2);
dev_dbg(pll->dev, "%s: VCO target rate %lu\n", name, rate * postdiv1 * postdiv2);
dev_dbg(pll->dev, "%s: Got fbdiv = %lu refdiv = %lu\n", name, best_fbdiv, best_refdiv);
setting = FIELD_PREP(ZX29_PLL_FBDIV_MASK, best_fbdiv);
setting |= FIELD_PREP(ZX29_PLL_REFDIV_MASK, best_refdiv);
setting |= FIELD_PREP(ZX29_PLL_POSTDIV1_MASK, postdiv1);
setting |= FIELD_PREP(ZX29_PLL_POSTDIV2_MASK, postdiv2);
dev_dbg(pll->dev, "%s: Final setting 0x%08x\n", name, setting);
return setting;
}
static int zx29_pll_determine_rate(struct clk_hw *hw, struct clk_rate_request *req)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
unsigned long new_rate;
u32 setting;
if (!req->best_parent_rate) {
dev_err(pll->dev, "Did not expect best_parent_rate=0\n");
return -EINVAL;
}
setting = zx29_pll_calc_values(pll, req->best_parent_rate, req->rate, req->min_rate,
req->max_rate);
if (!setting)
return -EINVAL;
new_rate = zx29_pll_get_rate(pll, req->best_parent_rate, setting);
if (new_rate != req->rate) {
dev_dbg(pll->dev, "Did not find an exact match. Want %lu, got %lu\n",
req->rate, new_rate);
req->rate = new_rate;
}
return 0;
}
static int zx29_pll_set_rate(struct clk_hw *hw, unsigned long rate,
unsigned long parent_rate)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
u32 setting;
setting = zx29_pll_calc_values(pll, parent_rate, rate, rate, rate);
if (!setting)
return -EINVAL;
dev_dbg(pll->dev, "%s: Setting new configuration: 0x%08x\n", clk_hw_get_name(hw), setting);
return regmap_update_bits(pll->map, pll->reg, 0x00ffffff, setting);
}
static u8 zx29_pll_get_parent(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
u32 val;
int res;
res = regmap_read(pll->map, pll->reg, &val);
if (res < 0)
return 0xff;
val = FIELD_GET(ZX29_PLL_PARENT_MASK, val);
dev_dbg(pll->dev, "%s: Parent 0x%x\n", clk_hw_get_name(hw), val);
return val;
}
static int zx29_pll_set_parent(struct clk_hw *hw, u8 index)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
u32 parent = FIELD_PREP(ZX29_PLL_PARENT_MASK, index);
int res;
u32 val;
res = regmap_update_bits(pll->map, pll->reg, ZX29_PLL_PARENT_MASK, parent);
if (res < 0)
return res;
res = regmap_read(pll->map, pll->reg, &val);
if (res < 0)
return res;
if ((val & ZX29_PLL_PARENT_MASK) != parent) {
dev_err(pll->dev, "Hardware rejected PLL parent %u\n", index);
return -EINVAL;
}
return 0;
}
static int zx29_pll_init(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
const char *name = clk_hw_get_name(hw);
int res;
u32 val;
dev_dbg(pll->dev, "%s: initializing\n", name);
res = regmap_read(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET, &val);
if (res < 0)
return res;
if (val & ZX29_PLL_FRACT) {
dev_warn(pll->dev, "%s: unsupported nonzero fractional value 0x%08lx\n",
name, val & ZX29_PLL_FRACT);
}
res = regmap_clear_bits(pll->map, pll->reg, ZX29_PLL_BYPASS);
if (res < 0)
return res;
res = regmap_test_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
if (res > 0) {
res = regmap_set_bits(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET,
ZX29_PLL_POSTDIV_OUT_DISABLE);
}
return res;
}
static const struct clk_ops zx29_pll_ops = {
.init = zx29_pll_init,
.is_prepared = zx29_pll_is_prepared,
.prepare = zx29_pll_prepare,
.unprepare = zx29_pll_unprepare,
.is_enabled = zx29_pll_is_enabled,
.enable = zx29_pll_enable,
.disable = zx29_pll_disable,
.recalc_rate = zx29_pll_recalc_rate,
.determine_rate = zx29_pll_determine_rate,
.get_parent = zx29_pll_get_parent,
.set_parent = zx29_pll_set_parent,
.set_rate = zx29_pll_set_rate,
};
static int zx29_pll_nodisable_init(struct clk_hw *hw)
{
struct zx29_clk_pll *pll = to_zx29_clk_pll(hw);
const char *name = clk_hw_get_name(hw);
int res;
u32 val;
dev_dbg(pll->dev, "%s: initializing, prepare-is-enabled\n", name);
res = regmap_read(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET, &val);
if (res < 0)
return res;
if (val & ZX29_PLL_FRACT) {
dev_warn(pll->dev, "%s: unsupported nonzero fractional value 0x%08lx\n",
name, val & ZX29_PLL_FRACT);
}
res = regmap_clear_bits(pll->map, pll->reg, ZX29_PLL_BYPASS);
if (res < 0)
return res;
return regmap_clear_bits(pll->map, pll->reg + ZX29_PLL_REG2_OFFSET,
ZX29_PLL_POSTDIV_OUT_DISABLE);
}
static const struct clk_ops zx29_pll_nodisable_ops = {
.init = zx29_pll_nodisable_init,
.is_prepared = zx29_pll_is_prepared,
.prepare = zx29_pll_prepare,
.unprepare = zx29_pll_unprepare,
.recalc_rate = zx29_pll_recalc_rate,
.determine_rate = zx29_pll_determine_rate,
.get_parent = zx29_pll_get_parent,
.set_parent = zx29_pll_set_parent,
.set_rate = zx29_pll_set_rate,
};
struct clk_hw *zx_clk_register_pll(struct device *dev, struct regmap *regmap,
const struct zx_pll_desc *desc, struct clk_hw * const *clocks)
{
struct clk_parent_data parents[CLK_ZX_MAX_PARENTS];
struct clk_init_data init = {};
struct zx29_clk_pll *pll;
unsigned int i;
int res;
pll = devm_kzalloc_obj(dev, *pll);
if (!pll)
return ERR_PTR(-ENOMEM);
if (desc->flags & CLK_ZX_PLL_PREPARE_IS_ENABLE)
init.ops = &zx29_pll_nodisable_ops;
else
init.ops = &zx29_pll_ops;
if (WARN_ON(desc->num_parents > ARRAY_SIZE(parents)))
return ERR_PTR(-EINVAL);
for (i = 0; i < desc->num_parents; ++i)
parents[i] = zx_get_parent(&desc->parents[i], clocks);
init.name = desc->name;
init.parent_data = parents;
init.num_parents = desc->num_parents;
init.flags = CLK_SET_PARENT_GATE;
pll->hw.init = &init;
pll->dev = dev;
pll->map = regmap;
pll->reg = desc->reg;
res = devm_clk_hw_register(dev, &pll->hw);
if (res)
return ERR_PTR(res);
res = regmap_test_bits(pll->map, pll->reg, ZX29_PLL_DISABLE);
if (res < 0)
return ERR_PTR(res);
if (res > 0 && desc->rate) {
dev_dbg(pll->dev, "%s: setting to %lu Hz\n", desc->name, desc->rate);
res = clk_set_rate(pll->hw.clk, desc->rate);
if (res)
return ERR_PTR(dev_err_probe(dev, res, "%s: failed to set rate\n",
desc->name));
}
return &pll->hw;
}