summaryrefslogtreecommitdiffstats
path: root/drivers/clk/st/clkgen-fsyn.c
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--drivers/clk/st/clkgen-fsyn.c975
1 files changed, 975 insertions, 0 deletions
diff --git a/drivers/clk/st/clkgen-fsyn.c b/drivers/clk/st/clkgen-fsyn.c
new file mode 100644
index 000000000..a79d81985
--- /dev/null
+++ b/drivers/clk/st/clkgen-fsyn.c
@@ -0,0 +1,975 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics R&D Ltd
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Authors:
+ * Stephen Gallimore <stephen.gallimore@st.com>,
+ * Pankaj Dev <pankaj.dev@st.com>.
+ */
+
+#include <linux/slab.h>
+#include <linux/of_address.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+
+#include "clkgen.h"
+
+/*
+ * Maximum input clock to the PLL before we divide it down by 2
+ * although in reality in actual systems this has never been seen to
+ * be used.
+ */
+#define QUADFS_NDIV_THRESHOLD 30000000
+
+#define PLL_BW_GOODREF (0L)
+#define PLL_BW_VBADREF (1L)
+#define PLL_BW_BADREF (2L)
+#define PLL_BW_VGOODREF (3L)
+
+#define QUADFS_MAX_CHAN 4
+
+struct stm_fs {
+ unsigned long ndiv;
+ unsigned long mdiv;
+ unsigned long pe;
+ unsigned long sdiv;
+ unsigned long nsdiv;
+};
+
+struct clkgen_quadfs_data {
+ bool reset_present;
+ bool bwfilter_present;
+ bool lockstatus_present;
+ bool powerup_polarity;
+ bool standby_polarity;
+ bool nsdiv_present;
+ bool nrst_present;
+ struct clkgen_field ndiv;
+ struct clkgen_field ref_bw;
+ struct clkgen_field nreset;
+ struct clkgen_field npda;
+ struct clkgen_field lock_status;
+
+ struct clkgen_field nrst[QUADFS_MAX_CHAN];
+ struct clkgen_field nsb[QUADFS_MAX_CHAN];
+ struct clkgen_field en[QUADFS_MAX_CHAN];
+ struct clkgen_field mdiv[QUADFS_MAX_CHAN];
+ struct clkgen_field pe[QUADFS_MAX_CHAN];
+ struct clkgen_field sdiv[QUADFS_MAX_CHAN];
+ struct clkgen_field nsdiv[QUADFS_MAX_CHAN];
+
+ const struct clk_ops *pll_ops;
+ int (*get_params)(unsigned long, unsigned long, struct stm_fs *);
+ int (*get_rate)(unsigned long , const struct stm_fs *,
+ unsigned long *);
+};
+
+static const struct clk_ops st_quadfs_pll_c32_ops;
+static const struct clk_ops st_quadfs_fs660c32_ops;
+
+static int clk_fs660c32_dig_get_params(unsigned long input,
+ unsigned long output, struct stm_fs *fs);
+static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *,
+ unsigned long *);
+
+static const struct clkgen_quadfs_data st_fs660c32_C = {
+ .nrst_present = true,
+ .nrst = { CLKGEN_FIELD(0x2f0, 0x1, 0),
+ CLKGEN_FIELD(0x2f0, 0x1, 1),
+ CLKGEN_FIELD(0x2f0, 0x1, 2),
+ CLKGEN_FIELD(0x2f0, 0x1, 3) },
+ .npda = CLKGEN_FIELD(0x2f0, 0x1, 12),
+ .nsb = { CLKGEN_FIELD(0x2f0, 0x1, 8),
+ CLKGEN_FIELD(0x2f0, 0x1, 9),
+ CLKGEN_FIELD(0x2f0, 0x1, 10),
+ CLKGEN_FIELD(0x2f0, 0x1, 11) },
+ .nsdiv_present = true,
+ .nsdiv = { CLKGEN_FIELD(0x304, 0x1, 24),
+ CLKGEN_FIELD(0x308, 0x1, 24),
+ CLKGEN_FIELD(0x30c, 0x1, 24),
+ CLKGEN_FIELD(0x310, 0x1, 24) },
+ .mdiv = { CLKGEN_FIELD(0x304, 0x1f, 15),
+ CLKGEN_FIELD(0x308, 0x1f, 15),
+ CLKGEN_FIELD(0x30c, 0x1f, 15),
+ CLKGEN_FIELD(0x310, 0x1f, 15) },
+ .en = { CLKGEN_FIELD(0x2fc, 0x1, 0),
+ CLKGEN_FIELD(0x2fc, 0x1, 1),
+ CLKGEN_FIELD(0x2fc, 0x1, 2),
+ CLKGEN_FIELD(0x2fc, 0x1, 3) },
+ .ndiv = CLKGEN_FIELD(0x2f4, 0x7, 16),
+ .pe = { CLKGEN_FIELD(0x304, 0x7fff, 0),
+ CLKGEN_FIELD(0x308, 0x7fff, 0),
+ CLKGEN_FIELD(0x30c, 0x7fff, 0),
+ CLKGEN_FIELD(0x310, 0x7fff, 0) },
+ .sdiv = { CLKGEN_FIELD(0x304, 0xf, 20),
+ CLKGEN_FIELD(0x308, 0xf, 20),
+ CLKGEN_FIELD(0x30c, 0xf, 20),
+ CLKGEN_FIELD(0x310, 0xf, 20) },
+ .lockstatus_present = true,
+ .lock_status = CLKGEN_FIELD(0x2f0, 0x1, 24),
+ .powerup_polarity = 1,
+ .standby_polarity = 1,
+ .pll_ops = &st_quadfs_pll_c32_ops,
+ .get_params = clk_fs660c32_dig_get_params,
+ .get_rate = clk_fs660c32_dig_get_rate,
+};
+
+static const struct clkgen_quadfs_data st_fs660c32_D = {
+ .nrst_present = true,
+ .nrst = { CLKGEN_FIELD(0x2a0, 0x1, 0),
+ CLKGEN_FIELD(0x2a0, 0x1, 1),
+ CLKGEN_FIELD(0x2a0, 0x1, 2),
+ CLKGEN_FIELD(0x2a0, 0x1, 3) },
+ .ndiv = CLKGEN_FIELD(0x2a4, 0x7, 16),
+ .pe = { CLKGEN_FIELD(0x2b4, 0x7fff, 0),
+ CLKGEN_FIELD(0x2b8, 0x7fff, 0),
+ CLKGEN_FIELD(0x2bc, 0x7fff, 0),
+ CLKGEN_FIELD(0x2c0, 0x7fff, 0) },
+ .sdiv = { CLKGEN_FIELD(0x2b4, 0xf, 20),
+ CLKGEN_FIELD(0x2b8, 0xf, 20),
+ CLKGEN_FIELD(0x2bc, 0xf, 20),
+ CLKGEN_FIELD(0x2c0, 0xf, 20) },
+ .npda = CLKGEN_FIELD(0x2a0, 0x1, 12),
+ .nsb = { CLKGEN_FIELD(0x2a0, 0x1, 8),
+ CLKGEN_FIELD(0x2a0, 0x1, 9),
+ CLKGEN_FIELD(0x2a0, 0x1, 10),
+ CLKGEN_FIELD(0x2a0, 0x1, 11) },
+ .nsdiv_present = true,
+ .nsdiv = { CLKGEN_FIELD(0x2b4, 0x1, 24),
+ CLKGEN_FIELD(0x2b8, 0x1, 24),
+ CLKGEN_FIELD(0x2bc, 0x1, 24),
+ CLKGEN_FIELD(0x2c0, 0x1, 24) },
+ .mdiv = { CLKGEN_FIELD(0x2b4, 0x1f, 15),
+ CLKGEN_FIELD(0x2b8, 0x1f, 15),
+ CLKGEN_FIELD(0x2bc, 0x1f, 15),
+ CLKGEN_FIELD(0x2c0, 0x1f, 15) },
+ .en = { CLKGEN_FIELD(0x2ac, 0x1, 0),
+ CLKGEN_FIELD(0x2ac, 0x1, 1),
+ CLKGEN_FIELD(0x2ac, 0x1, 2),
+ CLKGEN_FIELD(0x2ac, 0x1, 3) },
+ .lockstatus_present = true,
+ .lock_status = CLKGEN_FIELD(0x2A0, 0x1, 24),
+ .powerup_polarity = 1,
+ .standby_polarity = 1,
+ .pll_ops = &st_quadfs_pll_c32_ops,
+ .get_params = clk_fs660c32_dig_get_params,
+ .get_rate = clk_fs660c32_dig_get_rate,};
+
+/**
+ * DOC: A Frequency Synthesizer that multiples its input clock by a fixed factor
+ *
+ * Traits of this clock:
+ * prepare - clk_(un)prepare only ensures parent is (un)prepared
+ * enable - clk_enable and clk_disable are functional & control the Fsyn
+ * rate - inherits rate from parent. set_rate/round_rate/recalc_rate
+ * parent - fixed parent. No clk_set_parent support
+ */
+
+/**
+ * struct st_clk_quadfs_pll - A pll which outputs a fixed multiplier of
+ * its parent clock, found inside a type of
+ * ST quad channel frequency synthesizer block
+ *
+ * @hw: handle between common and hardware-specific interfaces.
+ * @ndiv: regmap field for the ndiv control.
+ * @regs_base: base address of the configuration registers.
+ * @lock: spinlock.
+ *
+ */
+struct st_clk_quadfs_pll {
+ struct clk_hw hw;
+ void __iomem *regs_base;
+ spinlock_t *lock;
+ struct clkgen_quadfs_data *data;
+ u32 ndiv;
+};
+
+#define to_quadfs_pll(_hw) container_of(_hw, struct st_clk_quadfs_pll, hw)
+
+static int quadfs_pll_enable(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
+ unsigned long flags = 0, timeout = jiffies + msecs_to_jiffies(10);
+
+ if (pll->lock)
+ spin_lock_irqsave(pll->lock, flags);
+
+ /*
+ * Bring block out of reset if we have reset control.
+ */
+ if (pll->data->reset_present)
+ CLKGEN_WRITE(pll, nreset, 1);
+
+ /*
+ * Use a fixed input clock noise bandwidth filter for the moment
+ */
+ if (pll->data->bwfilter_present)
+ CLKGEN_WRITE(pll, ref_bw, PLL_BW_GOODREF);
+
+
+ CLKGEN_WRITE(pll, ndiv, pll->ndiv);
+
+ /*
+ * Power up the PLL
+ */
+ CLKGEN_WRITE(pll, npda, !pll->data->powerup_polarity);
+
+ if (pll->lock)
+ spin_unlock_irqrestore(pll->lock, flags);
+
+ if (pll->data->lockstatus_present)
+ while (!CLKGEN_READ(pll, lock_status)) {
+ if (time_after(jiffies, timeout))
+ return -ETIMEDOUT;
+ cpu_relax();
+ }
+
+ return 0;
+}
+
+static void quadfs_pll_disable(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
+ unsigned long flags = 0;
+
+ if (pll->lock)
+ spin_lock_irqsave(pll->lock, flags);
+
+ /*
+ * Powerdown the PLL and then put block into soft reset if we have
+ * reset control.
+ */
+ CLKGEN_WRITE(pll, npda, pll->data->powerup_polarity);
+
+ if (pll->data->reset_present)
+ CLKGEN_WRITE(pll, nreset, 0);
+
+ if (pll->lock)
+ spin_unlock_irqrestore(pll->lock, flags);
+}
+
+static int quadfs_pll_is_enabled(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
+ u32 npda = CLKGEN_READ(pll, npda);
+
+ return pll->data->powerup_polarity ? !npda : !!npda;
+}
+
+static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
+ unsigned long *rate)
+{
+ unsigned long nd = fs->ndiv + 16; /* ndiv value */
+
+ *rate = input * nd;
+
+ return 0;
+}
+
+static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
+ unsigned long parent_rate)
+{
+ struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
+ unsigned long rate = 0;
+ struct stm_fs params;
+
+ params.ndiv = CLKGEN_READ(pll, ndiv);
+ if (clk_fs660c32_vco_get_rate(parent_rate, &params, &rate))
+ pr_err("%s:%s error calculating rate\n",
+ clk_hw_get_name(hw), __func__);
+
+ pll->ndiv = params.ndiv;
+
+ return rate;
+}
+
+static int clk_fs660c32_vco_get_params(unsigned long input,
+ unsigned long output, struct stm_fs *fs)
+{
+/* Formula
+ VCO frequency = (fin x ndiv) / pdiv
+ ndiv = VCOfreq * pdiv / fin
+ */
+ unsigned long pdiv = 1, n;
+
+ /* Output clock range: 384Mhz to 660Mhz */
+ if (output < 384000000 || output > 660000000)
+ return -EINVAL;
+
+ if (input > 40000000)
+ /* This means that PDIV would be 2 instead of 1.
+ Not supported today. */
+ return -EINVAL;
+
+ input /= 1000;
+ output /= 1000;
+
+ n = output * pdiv / input;
+ if (n < 16)
+ n = 16;
+ fs->ndiv = n - 16; /* Converting formula value to reg value */
+
+ return 0;
+}
+
+static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw,
+ unsigned long rate,
+ unsigned long *prate)
+{
+ struct stm_fs params;
+
+ if (clk_fs660c32_vco_get_params(*prate, rate, &params))
+ return rate;
+
+ clk_fs660c32_vco_get_rate(*prate, &params, &rate);
+
+ pr_debug("%s: %s new rate %ld [ndiv=%u]\n",
+ __func__, clk_hw_get_name(hw),
+ rate, (unsigned int)params.ndiv);
+
+ return rate;
+}
+
+static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
+ unsigned long parent_rate)
+{
+ struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
+ struct stm_fs params;
+ long hwrate = 0;
+ unsigned long flags = 0;
+ int ret;
+
+ if (!rate || !parent_rate)
+ return -EINVAL;
+
+ ret = clk_fs660c32_vco_get_params(parent_rate, rate, &params);
+ if (ret)
+ return ret;
+
+ clk_fs660c32_vco_get_rate(parent_rate, &params, &hwrate);
+
+ pr_debug("%s: %s new rate %ld [ndiv=0x%x]\n",
+ __func__, clk_hw_get_name(hw),
+ hwrate, (unsigned int)params.ndiv);
+
+ if (!hwrate)
+ return -EINVAL;
+
+ pll->ndiv = params.ndiv;
+
+ if (pll->lock)
+ spin_lock_irqsave(pll->lock, flags);
+
+ CLKGEN_WRITE(pll, ndiv, pll->ndiv);
+
+ if (pll->lock)
+ spin_unlock_irqrestore(pll->lock, flags);
+
+ return 0;
+}
+
+static const struct clk_ops st_quadfs_pll_c32_ops = {
+ .enable = quadfs_pll_enable,
+ .disable = quadfs_pll_disable,
+ .is_enabled = quadfs_pll_is_enabled,
+ .recalc_rate = quadfs_pll_fs660c32_recalc_rate,
+ .round_rate = quadfs_pll_fs660c32_round_rate,
+ .set_rate = quadfs_pll_fs660c32_set_rate,
+};
+
+static struct clk * __init st_clk_register_quadfs_pll(
+ const char *name, const char *parent_name,
+ struct clkgen_quadfs_data *quadfs, void __iomem *reg,
+ spinlock_t *lock)
+{
+ struct st_clk_quadfs_pll *pll;
+ struct clk *clk;
+ struct clk_init_data init;
+
+ /*
+ * Sanity check required pointers.
+ */
+ if (WARN_ON(!name || !parent_name))
+ return ERR_PTR(-EINVAL);
+
+ pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+ if (!pll)
+ return ERR_PTR(-ENOMEM);
+
+ init.name = name;
+ init.ops = quadfs->pll_ops;
+ init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
+ init.parent_names = &parent_name;
+ init.num_parents = 1;
+
+ pll->data = quadfs;
+ pll->regs_base = reg;
+ pll->lock = lock;
+ pll->hw.init = &init;
+
+ clk = clk_register(NULL, &pll->hw);
+
+ if (IS_ERR(clk))
+ kfree(pll);
+
+ return clk;
+}
+
+/**
+ * DOC: A digital frequency synthesizer
+ *
+ * Traits of this clock:
+ * prepare - clk_(un)prepare only ensures parent is (un)prepared
+ * enable - clk_enable and clk_disable are functional
+ * rate - set rate is functional
+ * parent - fixed parent. No clk_set_parent support
+ */
+
+/**
+ * struct st_clk_quadfs_fsynth - One clock output from a four channel digital
+ * frequency synthesizer (fsynth) block.
+ *
+ * @hw: handle between common and hardware-specific interfaces
+ *
+ * @nsb: regmap field in the output control register for the digital
+ * standby of this fsynth channel. This control is active low so
+ * the channel is in standby when the control bit is cleared.
+ *
+ * @nsdiv: regmap field in the output control register for
+ * for the optional divide by 3 of this fsynth channel. This control
+ * is active low so the divide by 3 is active when the control bit is
+ * cleared and the divide is bypassed when the bit is set.
+ */
+struct st_clk_quadfs_fsynth {
+ struct clk_hw hw;
+ void __iomem *regs_base;
+ spinlock_t *lock;
+ struct clkgen_quadfs_data *data;
+
+ u32 chan;
+ /*
+ * Cached hardware values from set_rate so we can program the
+ * hardware in enable. There are two reasons for this:
+ *
+ * 1. The registers may not be writable until the parent has been
+ * enabled.
+ *
+ * 2. It restores the clock rate when a driver does an enable
+ * on PM restore, after a suspend to RAM has lost the hardware
+ * setup.
+ */
+ u32 md;
+ u32 pe;
+ u32 sdiv;
+ u32 nsdiv;
+};
+
+#define to_quadfs_fsynth(_hw) \
+ container_of(_hw, struct st_clk_quadfs_fsynth, hw)
+
+static void quadfs_fsynth_program_enable(struct st_clk_quadfs_fsynth *fs)
+{
+ /*
+ * Pulse the program enable register lsb to make the hardware take
+ * notice of the new md/pe values with a glitchless transition.
+ */
+ CLKGEN_WRITE(fs, en[fs->chan], 1);
+ CLKGEN_WRITE(fs, en[fs->chan], 0);
+}
+
+static void quadfs_fsynth_program_rate(struct st_clk_quadfs_fsynth *fs)
+{
+ unsigned long flags = 0;
+
+ /*
+ * Ensure the md/pe parameters are ignored while we are
+ * reprogramming them so we can get a glitchless change
+ * when fine tuning the speed of a running clock.
+ */
+ CLKGEN_WRITE(fs, en[fs->chan], 0);
+
+ CLKGEN_WRITE(fs, mdiv[fs->chan], fs->md);
+ CLKGEN_WRITE(fs, pe[fs->chan], fs->pe);
+ CLKGEN_WRITE(fs, sdiv[fs->chan], fs->sdiv);
+
+ if (fs->lock)
+ spin_lock_irqsave(fs->lock, flags);
+
+ if (fs->data->nsdiv_present)
+ CLKGEN_WRITE(fs, nsdiv[fs->chan], fs->nsdiv);
+
+ if (fs->lock)
+ spin_unlock_irqrestore(fs->lock, flags);
+}
+
+static int quadfs_fsynth_enable(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ unsigned long flags = 0;
+
+ pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
+
+ quadfs_fsynth_program_rate(fs);
+
+ if (fs->lock)
+ spin_lock_irqsave(fs->lock, flags);
+
+ CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
+
+ if (fs->data->nrst_present)
+ CLKGEN_WRITE(fs, nrst[fs->chan], 0);
+
+ if (fs->lock)
+ spin_unlock_irqrestore(fs->lock, flags);
+
+ quadfs_fsynth_program_enable(fs);
+
+ return 0;
+}
+
+static void quadfs_fsynth_disable(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ unsigned long flags = 0;
+
+ pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
+
+ if (fs->lock)
+ spin_lock_irqsave(fs->lock, flags);
+
+ CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
+
+ if (fs->lock)
+ spin_unlock_irqrestore(fs->lock, flags);
+}
+
+static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]);
+
+ pr_debug("%s: %s enable bit = 0x%x\n",
+ __func__, clk_hw_get_name(hw), nsb);
+
+ return fs->data->standby_polarity ? !nsb : !!nsb;
+}
+
+#define P20 (uint64_t)(1 << 20)
+
+static int clk_fs660c32_dig_get_rate(unsigned long input,
+ const struct stm_fs *fs, unsigned long *rate)
+{
+ unsigned long s = (1 << fs->sdiv);
+ unsigned long ns;
+ uint64_t res;
+
+ /*
+ * 'nsdiv' is a register value ('BIN') which is translated
+ * to a decimal value according to following rules.
+ *
+ * nsdiv ns.dec
+ * 0 3
+ * 1 1
+ */
+ ns = (fs->nsdiv == 1) ? 1 : 3;
+
+ res = (P20 * (32 + fs->mdiv) + 32 * fs->pe) * s * ns;
+ *rate = (unsigned long)div64_u64(input * P20 * 32, res);
+
+ return 0;
+}
+
+
+static int clk_fs660c32_get_pe(int m, int si, unsigned long *deviation,
+ signed long input, unsigned long output, uint64_t *p,
+ struct stm_fs *fs)
+{
+ unsigned long new_freq, new_deviation;
+ struct stm_fs fs_tmp;
+ uint64_t val;
+
+ val = (uint64_t)output << si;
+
+ *p = (uint64_t)input * P20 - (32LL + (uint64_t)m) * val * (P20 / 32LL);
+
+ *p = div64_u64(*p, val);
+
+ if (*p > 32767LL)
+ return 1;
+
+ fs_tmp.mdiv = (unsigned long) m;
+ fs_tmp.pe = (unsigned long)*p;
+ fs_tmp.sdiv = si;
+ fs_tmp.nsdiv = 1;
+
+ clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
+
+ new_deviation = abs(output - new_freq);
+
+ if (new_deviation < *deviation) {
+ fs->mdiv = m;
+ fs->pe = (unsigned long)*p;
+ fs->sdiv = si;
+ fs->nsdiv = 1;
+ *deviation = new_deviation;
+ }
+ return 0;
+}
+
+static int clk_fs660c32_dig_get_params(unsigned long input,
+ unsigned long output, struct stm_fs *fs)
+{
+ int si; /* sdiv_reg (8 downto 0) */
+ int m; /* md value */
+ unsigned long new_freq, new_deviation;
+ /* initial condition to say: "infinite deviation" */
+ unsigned long deviation = ~0;
+ uint64_t p, p1, p2; /* pe value */
+ int r1, r2;
+
+ struct stm_fs fs_tmp;
+
+ for (si = 0; (si <= 8) && deviation; si++) {
+
+ /* Boundary test to avoid useless iteration */
+ r1 = clk_fs660c32_get_pe(0, si, &deviation,
+ input, output, &p1, fs);
+ r2 = clk_fs660c32_get_pe(31, si, &deviation,
+ input, output, &p2, fs);
+
+ /* No solution */
+ if (r1 && r2 && (p1 > p2))
+ continue;
+
+ /* Try to find best deviation */
+ for (m = 1; (m < 31) && deviation; m++)
+ clk_fs660c32_get_pe(m, si, &deviation,
+ input, output, &p, fs);
+
+ }
+
+ if (deviation == ~0) /* No solution found */
+ return -1;
+
+ /* pe fine tuning if deviation not 0: +/- 2 around computed pe value */
+ if (deviation) {
+ fs_tmp.mdiv = fs->mdiv;
+ fs_tmp.sdiv = fs->sdiv;
+ fs_tmp.nsdiv = fs->nsdiv;
+
+ if (fs->pe > 2)
+ p2 = fs->pe - 2;
+ else
+ p2 = 0;
+
+ for (; p2 < 32768ll && (p2 <= (fs->pe + 2)); p2++) {
+ fs_tmp.pe = (unsigned long)p2;
+
+ clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
+
+ new_deviation = abs(output - new_freq);
+
+ /* Check if this is a better solution */
+ if (new_deviation < deviation) {
+ fs->pe = (unsigned long)p2;
+ deviation = new_deviation;
+
+ }
+ }
+ }
+ return 0;
+}
+
+static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs,
+ struct stm_fs *params)
+{
+ /*
+ * Get the initial hardware values for recalc_rate
+ */
+ params->mdiv = CLKGEN_READ(fs, mdiv[fs->chan]);
+ params->pe = CLKGEN_READ(fs, pe[fs->chan]);
+ params->sdiv = CLKGEN_READ(fs, sdiv[fs->chan]);
+
+ if (fs->data->nsdiv_present)
+ params->nsdiv = CLKGEN_READ(fs, nsdiv[fs->chan]);
+ else
+ params->nsdiv = 1;
+
+ /*
+ * If All are NULL then assume no clock rate is programmed.
+ */
+ if (!params->mdiv && !params->pe && !params->sdiv)
+ return 1;
+
+ fs->md = params->mdiv;
+ fs->pe = params->pe;
+ fs->sdiv = params->sdiv;
+ fs->nsdiv = params->nsdiv;
+
+ return 0;
+}
+
+static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate,
+ unsigned long prate, struct stm_fs *params)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ int (*clk_fs_get_rate)(unsigned long ,
+ const struct stm_fs *, unsigned long *);
+ int (*clk_fs_get_params)(unsigned long, unsigned long, struct stm_fs *);
+ unsigned long rate = 0;
+
+ clk_fs_get_rate = fs->data->get_rate;
+ clk_fs_get_params = fs->data->get_params;
+
+ if (!clk_fs_get_params(prate, drate, params))
+ clk_fs_get_rate(prate, params, &rate);
+
+ return rate;
+}
+
+static unsigned long quadfs_recalc_rate(struct clk_hw *hw,
+ unsigned long parent_rate)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ unsigned long rate = 0;
+ struct stm_fs params;
+ int (*clk_fs_get_rate)(unsigned long ,
+ const struct stm_fs *, unsigned long *);
+
+ clk_fs_get_rate = fs->data->get_rate;
+
+ if (quadfs_fsynt_get_hw_value_for_recalc(fs, &params))
+ return 0;
+
+ if (clk_fs_get_rate(parent_rate, &params, &rate)) {
+ pr_err("%s:%s error calculating rate\n",
+ clk_hw_get_name(hw), __func__);
+ }
+
+ pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
+
+ return rate;
+}
+
+static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate,
+ unsigned long *prate)
+{
+ struct stm_fs params;
+
+ rate = quadfs_find_best_rate(hw, rate, *prate, &params);
+
+ pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
+ __func__, clk_hw_get_name(hw),
+ rate, (unsigned int)params.sdiv, (unsigned int)params.mdiv,
+ (unsigned int)params.pe, (unsigned int)params.nsdiv);
+
+ return rate;
+}
+
+
+static void quadfs_program_and_enable(struct st_clk_quadfs_fsynth *fs,
+ struct stm_fs *params)
+{
+ fs->md = params->mdiv;
+ fs->pe = params->pe;
+ fs->sdiv = params->sdiv;
+ fs->nsdiv = params->nsdiv;
+
+ /*
+ * In some integrations you can only change the fsynth programming when
+ * the parent entity containing it is enabled.
+ */
+ quadfs_fsynth_program_rate(fs);
+ quadfs_fsynth_program_enable(fs);
+}
+
+static int quadfs_set_rate(struct clk_hw *hw, unsigned long rate,
+ unsigned long parent_rate)
+{
+ struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
+ struct stm_fs params;
+ long hwrate;
+ int uninitialized_var(i);
+
+ if (!rate || !parent_rate)
+ return -EINVAL;
+
+ memset(&params, 0, sizeof(struct stm_fs));
+
+ hwrate = quadfs_find_best_rate(hw, rate, parent_rate, &params);
+ if (!hwrate)
+ return -EINVAL;
+
+ quadfs_program_and_enable(fs, &params);
+
+ return 0;
+}
+
+
+
+static const struct clk_ops st_quadfs_ops = {
+ .enable = quadfs_fsynth_enable,
+ .disable = quadfs_fsynth_disable,
+ .is_enabled = quadfs_fsynth_is_enabled,
+ .round_rate = quadfs_round_rate,
+ .set_rate = quadfs_set_rate,
+ .recalc_rate = quadfs_recalc_rate,
+};
+
+static struct clk * __init st_clk_register_quadfs_fsynth(
+ const char *name, const char *parent_name,
+ struct clkgen_quadfs_data *quadfs, void __iomem *reg, u32 chan,
+ unsigned long flags, spinlock_t *lock)
+{
+ struct st_clk_quadfs_fsynth *fs;
+ struct clk *clk;
+ struct clk_init_data init;
+
+ /*
+ * Sanity check required pointers, note that nsdiv3 is optional.
+ */
+ if (WARN_ON(!name || !parent_name))
+ return ERR_PTR(-EINVAL);
+
+ fs = kzalloc(sizeof(*fs), GFP_KERNEL);
+ if (!fs)
+ return ERR_PTR(-ENOMEM);
+
+ init.name = name;
+ init.ops = &st_quadfs_ops;
+ init.flags = flags | CLK_GET_RATE_NOCACHE | CLK_IS_BASIC;
+ init.parent_names = &parent_name;
+ init.num_parents = 1;
+
+ fs->data = quadfs;
+ fs->regs_base = reg;
+ fs->chan = chan;
+ fs->lock = lock;
+ fs->hw.init = &init;
+
+ clk = clk_register(NULL, &fs->hw);
+
+ if (IS_ERR(clk))
+ kfree(fs);
+
+ return clk;
+}
+
+static void __init st_of_create_quadfs_fsynths(
+ struct device_node *np, const char *pll_name,
+ struct clkgen_quadfs_data *quadfs, void __iomem *reg,
+ spinlock_t *lock)
+{
+ struct clk_onecell_data *clk_data;
+ int fschan;
+
+ clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
+ if (!clk_data)
+ return;
+
+ clk_data->clk_num = QUADFS_MAX_CHAN;
+ clk_data->clks = kcalloc(QUADFS_MAX_CHAN, sizeof(struct clk *),
+ GFP_KERNEL);
+
+ if (!clk_data->clks) {
+ kfree(clk_data);
+ return;
+ }
+
+ for (fschan = 0; fschan < QUADFS_MAX_CHAN; fschan++) {
+ struct clk *clk;
+ const char *clk_name;
+ unsigned long flags = 0;
+
+ if (of_property_read_string_index(np, "clock-output-names",
+ fschan, &clk_name)) {
+ break;
+ }
+
+ /*
+ * If we read an empty clock name then the channel is unused
+ */
+ if (*clk_name == '\0')
+ continue;
+
+ of_clk_detect_critical(np, fschan, &flags);
+
+ clk = st_clk_register_quadfs_fsynth(clk_name, pll_name,
+ quadfs, reg, fschan,
+ flags, lock);
+
+ /*
+ * If there was an error registering this clock output, clean
+ * up and move on to the next one.
+ */
+ if (!IS_ERR(clk)) {
+ clk_data->clks[fschan] = clk;
+ pr_debug("%s: parent %s rate %u\n",
+ __clk_get_name(clk),
+ __clk_get_name(clk_get_parent(clk)),
+ (unsigned int)clk_get_rate(clk));
+ }
+ }
+
+ of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
+}
+
+static void __init st_of_quadfs_setup(struct device_node *np,
+ struct clkgen_quadfs_data *data)
+{
+ struct clk *clk;
+ const char *pll_name, *clk_parent_name;
+ void __iomem *reg;
+ spinlock_t *lock;
+
+ reg = of_iomap(np, 0);
+ if (!reg)
+ return;
+
+ clk_parent_name = of_clk_get_parent_name(np, 0);
+ if (!clk_parent_name)
+ return;
+
+ pll_name = kasprintf(GFP_KERNEL, "%s.pll", np->name);
+ if (!pll_name)
+ return;
+
+ lock = kzalloc(sizeof(*lock), GFP_KERNEL);
+ if (!lock)
+ goto err_exit;
+
+ spin_lock_init(lock);
+
+ clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data,
+ reg, lock);
+ if (IS_ERR(clk))
+ goto err_exit;
+ else
+ pr_debug("%s: parent %s rate %u\n",
+ __clk_get_name(clk),
+ __clk_get_name(clk_get_parent(clk)),
+ (unsigned int)clk_get_rate(clk));
+
+ st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock);
+
+err_exit:
+ kfree(pll_name); /* No longer need local copy of the PLL name */
+}
+
+static void __init st_of_quadfs660C_setup(struct device_node *np)
+{
+ st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C);
+}
+CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup);
+
+static void __init st_of_quadfs660D_setup(struct device_node *np)
+{
+ st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D);
+}
+CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup);
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-04 11:41:50 +0000