// SPDX-License-Identifier: GPL-2.0-only /* * Purna Chandra Mandal, * Copyright (C) 2015 Microchip Technology Inc. All rights reserved. */ #include #include #include #include #include #include #include #include #include "clk-core.h" /* OSCCON Reg fields */ #define OSC_CUR_MASK 0x07 #define OSC_CUR_SHIFT 12 #define OSC_NEW_MASK 0x07 #define OSC_NEW_SHIFT 8 #define OSC_SWEN BIT(0) /* SPLLCON Reg fields */ #define PLL_RANGE_MASK 0x07 #define PLL_RANGE_SHIFT 0 #define PLL_ICLK_MASK 0x01 #define PLL_ICLK_SHIFT 7 #define PLL_IDIV_MASK 0x07 #define PLL_IDIV_SHIFT 8 #define PLL_ODIV_MASK 0x07 #define PLL_ODIV_SHIFT 24 #define PLL_MULT_MASK 0x7F #define PLL_MULT_SHIFT 16 #define PLL_MULT_MAX 128 #define PLL_ODIV_MIN 1 #define PLL_ODIV_MAX 5 /* Peripheral Bus Clock Reg Fields */ #define PB_DIV_MASK 0x7f #define PB_DIV_SHIFT 0 #define PB_DIV_READY BIT(11) #define PB_DIV_ENABLE BIT(15) #define PB_DIV_MAX 128 #define PB_DIV_MIN 0 /* Reference Oscillator Control Reg fields */ #define REFO_SEL_MASK 0x0f #define REFO_SEL_SHIFT 0 #define REFO_ACTIVE BIT(8) #define REFO_DIVSW_EN BIT(9) #define REFO_OE BIT(12) #define REFO_ON BIT(15) #define REFO_DIV_SHIFT 16 #define REFO_DIV_MASK 0x7fff /* Reference Oscillator Trim Register Fields */ #define REFO_TRIM_REG 0x10 #define REFO_TRIM_MASK 0x1ff #define REFO_TRIM_SHIFT 23 #define REFO_TRIM_MAX 511 /* Mux Slew Control Register fields */ #define SLEW_BUSY BIT(0) #define SLEW_DOWNEN BIT(1) #define SLEW_UPEN BIT(2) #define SLEW_DIV 0x07 #define SLEW_DIV_SHIFT 8 #define SLEW_SYSDIV 0x0f #define SLEW_SYSDIV_SHIFT 20 /* Clock Poll Timeout */ #define LOCK_TIMEOUT_US USEC_PER_MSEC /* SoC specific clock needed during SPLL clock rate switch */ static struct clk_hw *pic32_sclk_hw; /* add instruction pipeline delay while CPU clock is in-transition. */ #define cpu_nop5() \ do { \ __asm__ __volatile__("nop"); \ __asm__ __volatile__("nop"); \ __asm__ __volatile__("nop"); \ __asm__ __volatile__("nop"); \ __asm__ __volatile__("nop"); \ } while (0) /* Perpheral bus clocks */ struct pic32_periph_clk { struct clk_hw hw; void __iomem *ctrl_reg; struct pic32_clk_common *core; }; #define clkhw_to_pbclk(_hw) container_of(_hw, struct pic32_periph_clk, hw) static int pbclk_is_enabled(struct clk_hw *hw) { struct pic32_periph_clk *pb = clkhw_to_pbclk(hw); return readl(pb->ctrl_reg) & PB_DIV_ENABLE; } static int pbclk_enable(struct clk_hw *hw) { struct pic32_periph_clk *pb = clkhw_to_pbclk(hw); writel(PB_DIV_ENABLE, PIC32_SET(pb->ctrl_reg)); return 0; } static void pbclk_disable(struct clk_hw *hw) { struct pic32_periph_clk *pb = clkhw_to_pbclk(hw); writel(PB_DIV_ENABLE, PIC32_CLR(pb->ctrl_reg)); } static unsigned long calc_best_divided_rate(unsigned long rate, unsigned long parent_rate, u32 divider_max, u32 divider_min) { unsigned long divided_rate, divided_rate_down, best_rate; unsigned long div, div_up; /* eq. clk_rate = parent_rate / divider. * * Find best divider to produce closest of target divided rate. */ div = parent_rate / rate; div = clamp_val(div, divider_min, divider_max); div_up = clamp_val(div + 1, divider_min, divider_max); divided_rate = parent_rate / div; divided_rate_down = parent_rate / div_up; if (abs(rate - divided_rate_down) < abs(rate - divided_rate)) best_rate = divided_rate_down; else best_rate = divided_rate; return best_rate; } static inline u32 pbclk_read_pbdiv(struct pic32_periph_clk *pb) { return ((readl(pb->ctrl_reg) >> PB_DIV_SHIFT) & PB_DIV_MASK) + 1; } static unsigned long pbclk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) { struct pic32_periph_clk *pb = clkhw_to_pbclk(hw); return parent_rate / pbclk_read_pbdiv(pb); } static long pbclk_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *parent_rate) { return calc_best_divided_rate(rate, *parent_rate, PB_DIV_MAX, PB_DIV_MIN); } static int pbclk_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { struct pic32_periph_clk *pb = clkhw_to_pbclk(hw); unsigned long flags; u32 v, div; int err; /* check & wait for DIV_READY */ err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY, 1, LOCK_TIMEOUT_US); if (err) return err; /* calculate clkdiv and best rate */ div = DIV_ROUND_CLOSEST(parent_rate, rate); spin_lock_irqsave(&pb->core->reg_lock, flags); /* apply new div */ v = readl(pb->ctrl_reg); v &= ~PB_DIV_MASK; v |= (div - 1); pic32_syskey_unlock(); writel(v, pb->ctrl_reg); spin_unlock_irqrestore(&pb->core->reg_lock, flags); /* wait again for DIV_READY */ err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY, 1, LOCK_TIMEOUT_US); if (err) return err; /* confirm that new div is applied correctly */ return (pbclk_read_pbdiv(pb) == div) ? 0 : -EBUSY; } const struct clk_ops pic32_pbclk_ops = { .enable = pbclk_enable, .disable = pbclk_disable, .is_enabled = pbclk_is_enabled, .recalc_rate = pbclk_recalc_rate, .round_rate = pbclk_round_rate, .set_rate = pbclk_set_rate, }; struct clk *pic32_periph_clk_register(const struct pic32_periph_clk_data *desc, struct pic32_clk_common *core) { struct pic32_periph_clk *pbclk; struct clk *clk; pbclk = devm_kzalloc(core->dev, sizeof(*pbclk), GFP_KERNEL); if (!pbclk) return ERR_PTR(-ENOMEM); pbclk->hw.init = &desc->init_data; pbclk->core = core; pbclk->ctrl_reg = desc->ctrl_reg + core->iobase; clk = devm_clk_register(core->dev, &pbclk->hw); if (IS_ERR(clk)) { dev_err(core->dev, "%s: clk_register() failed\n", __func__); devm_kfree(core->dev, pbclk); } return clk; } /* Reference oscillator operations */ struct pic32_ref_osc { struct clk_hw hw; void __iomem *ctrl_reg; const u32 *parent_map; struct pic32_clk_common *core; }; #define clkhw_to_refosc(_hw) container_of(_hw, struct pic32_ref_osc, hw) static int roclk_is_enabled(struct clk_hw *hw) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); return readl(refo->ctrl_reg) & REFO_ON; } static int roclk_enable(struct clk_hw *hw) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); writel(REFO_ON | REFO_OE, PIC32_SET(refo->ctrl_reg)); return 0; } static void roclk_disable(struct clk_hw *hw) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); writel(REFO_ON | REFO_OE, PIC32_CLR(refo->ctrl_reg)); } static int roclk_init(struct clk_hw *hw) { /* initialize clock in disabled state */ roclk_disable(hw); return 0; } static u8 roclk_get_parent(struct clk_hw *hw) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); u32 v, i; v = (readl(refo->ctrl_reg) >> REFO_SEL_SHIFT) & REFO_SEL_MASK; if (!refo->parent_map) return v; for (i = 0; i < clk_hw_get_num_parents(hw); i++) if (refo->parent_map[i] == v) return i; return -EINVAL; } static unsigned long roclk_calc_rate(unsigned long parent_rate, u32 rodiv, u32 rotrim) { u64 rate64; /* fout = fin / [2 * {div + (trim / 512)}] * = fin * 512 / [1024 * div + 2 * trim] * = fin * 256 / (512 * div + trim) * = (fin << 8) / ((div << 9) + trim) */ if (rotrim) { rodiv = (rodiv << 9) + rotrim; rate64 = parent_rate; rate64 <<= 8; do_div(rate64, rodiv); } else if (rodiv) { rate64 = parent_rate / (rodiv << 1); } else { rate64 = parent_rate; } return rate64; } static void roclk_calc_div_trim(unsigned long rate, unsigned long parent_rate, u32 *rodiv_p, u32 *rotrim_p) { u32 div, rotrim, rodiv; u64 frac; /* Find integer approximation of floating-point arithmetic. * fout = fin / [2 * {rodiv + (rotrim / 512)}] ... (1) * i.e. fout = fin / 2 * DIV * whereas DIV = rodiv + (rotrim / 512) * * Since kernel does not perform floating-point arithmatic so * (rotrim/512) will be zero. And DIV & rodiv will result same. * * ie. fout = (fin * 256) / [(512 * rodiv) + rotrim] ... from (1) * ie. rotrim = ((fin * 256) / fout) - (512 * DIV) */ if (parent_rate <= rate) { div = 0; frac = 0; rodiv = 0; rotrim = 0; } else { div = parent_rate / (rate << 1); frac = parent_rate; frac <<= 8; do_div(frac, rate); frac -= (u64)(div << 9); rodiv = (div > REFO_DIV_MASK) ? REFO_DIV_MASK : div; rotrim = (frac >= REFO_TRIM_MAX) ? REFO_TRIM_MAX : frac; } if (rodiv_p) *rodiv_p = rodiv; if (rotrim_p) *rotrim_p = rotrim; } static unsigned long roclk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); u32 v, rodiv, rotrim; /* get rodiv */ v = readl(refo->ctrl_reg); rodiv = (v >> REFO_DIV_SHIFT) & REFO_DIV_MASK; /* get trim */ v = readl(refo->ctrl_reg + REFO_TRIM_REG); rotrim = (v >> REFO_TRIM_SHIFT) & REFO_TRIM_MASK; return roclk_calc_rate(parent_rate, rodiv, rotrim); } static long roclk_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *parent_rate) { u32 rotrim, rodiv; /* calculate dividers for new rate */ roclk_calc_div_trim(rate, *parent_rate, &rodiv, &rotrim); /* caclulate new rate (rounding) based on new rodiv & rotrim */ return roclk_calc_rate(*parent_rate, rodiv, rotrim); } static int roclk_determine_rate(struct clk_hw *hw, struct clk_rate_request *req) { struct clk_hw *parent_clk, *best_parent_clk = NULL; unsigned int i, delta, best_delta = -1; unsigned long parent_rate, best_parent_rate = 0; unsigned long best = 0, nearest_rate; /* find a parent which can generate nearest clkrate >= rate */ for (i = 0; i < clk_hw_get_num_parents(hw); i++) { /* get parent */ parent_clk = clk_hw_get_parent_by_index(hw, i); if (!parent_clk) continue; /* skip if parent runs slower than target rate */ parent_rate = clk_hw_get_rate(parent_clk); if (req->rate > parent_rate) continue; nearest_rate = roclk_round_rate(hw, req->rate, &parent_rate); delta = abs(nearest_rate - req->rate); if ((nearest_rate >= req->rate) && (delta < best_delta)) { best_parent_clk = parent_clk; best_parent_rate = parent_rate; best = nearest_rate; best_delta = delta; if (delta == 0) break; } } /* if no match found, retain old rate */ if (!best_parent_clk) { pr_err("%s:%s, no parent found for rate %lu.\n", __func__, clk_hw_get_name(hw), req->rate); return clk_hw_get_rate(hw); } pr_debug("%s,rate %lu, best_parent(%s, %lu), best %lu, delta %d\n", clk_hw_get_name(hw), req->rate, clk_hw_get_name(best_parent_clk), best_parent_rate, best, best_delta); if (req->best_parent_rate) req->best_parent_rate = best_parent_rate; if (req->best_parent_hw) req->best_parent_hw = best_parent_clk; return best; } static int roclk_set_parent(struct clk_hw *hw, u8 index) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); unsigned long flags; u32 v; int err; if (refo->parent_map) index = refo->parent_map[index]; /* wait until ACTIVE bit is zero or timeout */ err = readl_poll_timeout(refo->ctrl_reg, v, !(v & REFO_ACTIVE), 1, LOCK_TIMEOUT_US); if (err) { pr_err("%s: poll failed, clk active\n", clk_hw_get_name(hw)); return err; } spin_lock_irqsave(&refo->core->reg_lock, flags); pic32_syskey_unlock(); /* calculate & apply new */ v = readl(refo->ctrl_reg); v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT); v |= index << REFO_SEL_SHIFT; writel(v, refo->ctrl_reg); spin_unlock_irqrestore(&refo->core->reg_lock, flags); return 0; } static int roclk_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate, u8 index) { struct pic32_ref_osc *refo = clkhw_to_refosc(hw); unsigned long flags; u32 trim, rodiv, v; int err; /* calculate new rodiv & rotrim for new rate */ roclk_calc_div_trim(rate, parent_rate, &rodiv, &trim); pr_debug("parent_rate = %lu, rate = %lu, div = %d, trim = %d\n", parent_rate, rate, rodiv, trim); /* wait till source change is active */ err = readl_poll_timeout(refo->ctrl_reg, v, !(v & (REFO_ACTIVE | REFO_DIVSW_EN)), 1, LOCK_TIMEOUT_US); if (err) { pr_err("%s: poll timedout, clock is still active\n", __func__); return err; } spin_lock_irqsave(&refo->core->reg_lock, flags); v = readl(refo->ctrl_reg); pic32_syskey_unlock(); /* apply parent, if required */ if (refo->parent_map) index = refo->parent_map[index]; v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT); v |= index << REFO_SEL_SHIFT; /* apply RODIV */ v &= ~(REFO_DIV_MASK << REFO_DIV_SHIFT); v |= rodiv << REFO_DIV_SHIFT; writel(v, refo->ctrl_reg); /* apply ROTRIM */ v = readl(refo->ctrl_reg + REFO_TRIM_REG); v &= ~(REFO_TRIM_MASK << REFO_TRIM_SHIFT); v |= trim << REFO_TRIM_SHIFT; writel(v, refo->ctrl_reg + REFO_TRIM_REG); /* enable & activate divider switching */ writel(REFO_ON | REFO_DIVSW_EN, PIC32_SET(refo->ctrl_reg)); /* wait till divswen is in-progress */ err = readl_poll_timeout_atomic(refo->ctrl_reg, v, !(v & REFO_DIVSW_EN), 1, LOCK_TIMEOUT_US); /* leave the clk gated as it was */ writel(REFO_ON, PIC32_CLR(refo->ctrl_reg)); spin_unlock_irqrestore(&refo->core->reg_lock, flags); return err; } static int roclk_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { u8 index = roclk_get_parent(hw); return roclk_set_rate_and_parent(hw, rate, parent_rate, index); } const struct clk_ops pic32_roclk_ops = { .enable = roclk_enable, .disable = roclk_disable, .is_enabled = roclk_is_enabled, .get_parent = roclk_get_parent, .set_parent = roclk_set_parent, .determine_rate = roclk_determine_rate, .recalc_rate = roclk_recalc_rate, .set_rate_and_parent = roclk_set_rate_and_parent, .set_rate = roclk_set_rate, .init = roclk_init, }; struct clk *pic32_refo_clk_register(const struct pic32_ref_osc_data *data, struct pic32_clk_common *core) { struct pic32_ref_osc *refo; struct clk *clk; refo = devm_kzalloc(core->dev, sizeof(*refo), GFP_KERNEL); if (!refo) return ERR_PTR(-ENOMEM); refo->core = core; refo->hw.init = &data->init_data; refo->ctrl_reg = data->ctrl_reg + core->iobase; refo->parent_map = data->parent_map; clk = devm_clk_register(core->dev, &refo->hw); if (IS_ERR(clk)) dev_err(core->dev, "%s: clk_register() failed\n", __func__); return clk; } struct pic32_sys_pll { struct clk_hw hw; void __iomem *ctrl_reg; void __iomem *status_reg; u32 lock_mask; u32 idiv; /* PLL iclk divider, treated fixed */ struct pic32_clk_common *core; }; #define clkhw_to_spll(_hw) container_of(_hw, struct pic32_sys_pll, hw) static inline u32 spll_odiv_to_divider(u32 odiv) { odiv = clamp_val(odiv, PLL_ODIV_MIN, PLL_ODIV_MAX); return 1 << odiv; } static unsigned long spll_calc_mult_div(struct pic32_sys_pll *pll, unsigned long rate, unsigned long parent_rate, u32 *mult_p, u32 *odiv_p) { u32 mul, div, best_mul = 1, best_div = 1; unsigned long new_rate, best_rate = rate; unsigned int best_delta = -1, delta, match_found = 0; u64 rate64; parent_rate /= pll->idiv; for (mul = 1; mul <= PLL_MULT_MAX; mul++) { for (div = PLL_ODIV_MIN; div <= PLL_ODIV_MAX; div++) { rate64 = parent_rate; rate64 *= mul; do_div(rate64, 1 << div); new_rate = rate64; delta = abs(rate - new_rate); if ((new_rate >= rate) && (delta < best_delta)) { best_delta = delta; best_rate = new_rate; best_mul = mul; best_div = div; match_found = 1; } } } if (!match_found) { pr_warn("spll: no match found\n"); return 0; } pr_debug("rate %lu, par_rate %lu/mult %u, div %u, best_rate %lu\n", rate, parent_rate, best_mul, best_div, best_rate); if (mult_p) *mult_p = best_mul - 1; if (odiv_p) *odiv_p = best_div; return best_rate; } static unsigned long spll_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) { struct pic32_sys_pll *pll = clkhw_to_spll(hw); unsigned long pll_in_rate; u32 mult, odiv, div, v; u64 rate64; v = readl(pll->ctrl_reg); odiv = ((v >> PLL_ODIV_SHIFT) & PLL_ODIV_MASK); mult = ((v >> PLL_MULT_SHIFT) & PLL_MULT_MASK) + 1; div = spll_odiv_to_divider(odiv); /* pll_in_rate = parent_rate / idiv * pll_out_rate = pll_in_rate * mult / div; */ pll_in_rate = parent_rate / pll->idiv; rate64 = pll_in_rate; rate64 *= mult; do_div(rate64, div); return rate64; } static long spll_clk_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *parent_rate) { struct pic32_sys_pll *pll = clkhw_to_spll(hw); return spll_calc_mult_div(pll, rate, *parent_rate, NULL, NULL); } static int spll_clk_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { struct pic32_sys_pll *pll = clkhw_to_spll(hw); unsigned long ret, flags; u32 mult, odiv, v; int err; ret = spll_calc_mult_div(pll, rate, parent_rate, &mult, &odiv); if (!ret) return -EINVAL; /* * We can't change SPLL counters when it is in-active use * by SYSCLK. So check before applying new counters/rate. */ /* Is spll_clk active parent of sys_clk ? */ if (unlikely(clk_hw_get_parent(pic32_sclk_hw) == hw)) { pr_err("%s: failed, clk in-use\n", __func__); return -EBUSY; } spin_lock_irqsave(&pll->core->reg_lock, flags); /* apply new multiplier & divisor */ v = readl(pll->ctrl_reg); v &= ~(PLL_MULT_MASK << PLL_MULT_SHIFT); v &= ~(PLL_ODIV_MASK << PLL_ODIV_SHIFT); v |= (mult << PLL_MULT_SHIFT) | (odiv << PLL_ODIV_SHIFT); /* sys unlock before write */ pic32_syskey_unlock(); writel(v, pll->ctrl_reg); cpu_relax(); /* insert few nops (5-stage) to ensure CPU does not hang */ cpu_nop5(); cpu_nop5(); /* Wait until PLL is locked (maximum 100 usecs). */ err = readl_poll_timeout_atomic(pll->status_reg, v, v & pll->lock_mask, 1, 100); spin_unlock_irqrestore(&pll->core->reg_lock, flags); return err; } /* SPLL clock operation */ const struct clk_ops pic32_spll_ops = { .recalc_rate = spll_clk_recalc_rate, .round_rate = spll_clk_round_rate, .set_rate = spll_clk_set_rate, }; struct clk *pic32_spll_clk_register(const struct pic32_sys_pll_data *data, struct pic32_clk_common *core) { struct pic32_sys_pll *spll; struct clk *clk; spll = devm_kzalloc(core->dev, sizeof(*spll), GFP_KERNEL); if (!spll) return ERR_PTR(-ENOMEM); spll->core = core; spll->hw.init = &data->init_data; spll->ctrl_reg = data->ctrl_reg + core->iobase; spll->status_reg = data->status_reg + core->iobase; spll->lock_mask = data->lock_mask; /* cache PLL idiv; PLL driver uses it as constant.*/ spll->idiv = (readl(spll->ctrl_reg) >> PLL_IDIV_SHIFT) & PLL_IDIV_MASK; spll->idiv += 1; clk = devm_clk_register(core->dev, &spll->hw); if (IS_ERR(clk)) dev_err(core->dev, "sys_pll: clk_register() failed\n"); return clk; } /* System mux clock(aka SCLK) */ struct pic32_sys_clk { struct clk_hw hw; void __iomem *mux_reg; void __iomem *slew_reg; u32 slew_div; const u32 *parent_map; struct pic32_clk_common *core; }; #define clkhw_to_sys_clk(_hw) container_of(_hw, struct pic32_sys_clk, hw) static unsigned long sclk_get_rate(struct clk_hw *hw, unsigned long parent_rate) { struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw); u32 div; div = (readl(sclk->slew_reg) >> SLEW_SYSDIV_SHIFT) & SLEW_SYSDIV; div += 1; /* sys-div to divider */ return parent_rate / div; } static long sclk_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *parent_rate) { return calc_best_divided_rate(rate, *parent_rate, SLEW_SYSDIV, 1); } static int sclk_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw); unsigned long flags; u32 v, div; int err; div = parent_rate / rate; spin_lock_irqsave(&sclk->core->reg_lock, flags); /* apply new div */ v = readl(sclk->slew_reg); v &= ~(SLEW_SYSDIV << SLEW_SYSDIV_SHIFT); v |= (div - 1) << SLEW_SYSDIV_SHIFT; pic32_syskey_unlock(); writel(v, sclk->slew_reg); /* wait until BUSY is cleared */ err = readl_poll_timeout_atomic(sclk->slew_reg, v, !(v & SLEW_BUSY), 1, LOCK_TIMEOUT_US); spin_unlock_irqrestore(&sclk->core->reg_lock, flags); return err; } static u8 sclk_get_parent(struct clk_hw *hw) { struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw); u32 i, v; v = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK; if (!sclk->parent_map) return v; for (i = 0; i < clk_hw_get_num_parents(hw); i++) if (sclk->parent_map[i] == v) return i; return -EINVAL; } static int sclk_set_parent(struct clk_hw *hw, u8 index) { struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw); unsigned long flags; u32 nosc, cosc, v; int err; spin_lock_irqsave(&sclk->core->reg_lock, flags); /* find new_osc */ nosc = sclk->parent_map ? sclk->parent_map[index] : index; /* set new parent */ v = readl(sclk->mux_reg); v &= ~(OSC_NEW_MASK << OSC_NEW_SHIFT); v |= nosc << OSC_NEW_SHIFT; pic32_syskey_unlock(); writel(v, sclk->mux_reg); /* initate switch */ writel(OSC_SWEN, PIC32_SET(sclk->mux_reg)); cpu_relax(); /* add nop to flush pipeline (as cpu_clk is in-flux) */ cpu_nop5(); /* wait for SWEN bit to clear */ err = readl_poll_timeout_atomic(sclk->slew_reg, v, !(v & OSC_SWEN), 1, LOCK_TIMEOUT_US); spin_unlock_irqrestore(&sclk->core->reg_lock, flags); /* * SCLK clock-switching logic might reject a clock switching request * if pre-requisites (like new clk_src not present or unstable) are * not met. * So confirm before claiming success. */ cosc = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK; if (cosc != nosc) { pr_err("%s: err, failed to set_parent() to %d, current %d\n", clk_hw_get_name(hw), nosc, cosc); err = -EBUSY; } return err; } static int sclk_init(struct clk_hw *hw) { struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw); unsigned long flags; u32 v; /* Maintain reference to this clk, required in spll_clk_set_rate() */ pic32_sclk_hw = hw; /* apply slew divider on both up and down scaling */ if (sclk->slew_div) { spin_lock_irqsave(&sclk->core->reg_lock, flags); v = readl(sclk->slew_reg); v &= ~(SLEW_DIV << SLEW_DIV_SHIFT); v |= sclk->slew_div << SLEW_DIV_SHIFT; v |= SLEW_DOWNEN | SLEW_UPEN; writel(v, sclk->slew_reg); spin_unlock_irqrestore(&sclk->core->reg_lock, flags); } return 0; } /* sclk with post-divider */ const struct clk_ops pic32_sclk_ops = { .get_parent = sclk_get_parent, .set_parent = sclk_set_parent, .round_rate = sclk_round_rate, .set_rate = sclk_set_rate, .recalc_rate = sclk_get_rate, .init = sclk_init, .determine_rate = __clk_mux_determine_rate, }; /* sclk with no slew and no post-divider */ const struct clk_ops pic32_sclk_no_div_ops = { .get_parent = sclk_get_parent, .set_parent = sclk_set_parent, .init = sclk_init, .determine_rate = __clk_mux_determine_rate, }; struct clk *pic32_sys_clk_register(const struct pic32_sys_clk_data *data, struct pic32_clk_common *core) { struct pic32_sys_clk *sclk; struct clk *clk; sclk = devm_kzalloc(core->dev, sizeof(*sclk), GFP_KERNEL); if (!sclk) return ERR_PTR(-ENOMEM); sclk->core = core; sclk->hw.init = &data->init_data; sclk->mux_reg = data->mux_reg + core->iobase; sclk->slew_reg = data->slew_reg + core->iobase; sclk->slew_div = data->slew_div; sclk->parent_map = data->parent_map; clk = devm_clk_register(core->dev, &sclk->hw); if (IS_ERR(clk)) dev_err(core->dev, "%s: clk register failed\n", __func__); return clk; } /* secondary oscillator */ struct pic32_sec_osc { struct clk_hw hw; void __iomem *enable_reg; void __iomem *status_reg; u32 enable_mask; u32 status_mask; unsigned long fixed_rate; struct pic32_clk_common *core; }; #define clkhw_to_sosc(_hw) container_of(_hw, struct pic32_sec_osc, hw) static int sosc_clk_enable(struct clk_hw *hw) { struct pic32_sec_osc *sosc = clkhw_to_sosc(hw); u32 v; /* enable SOSC */ pic32_syskey_unlock(); writel(sosc->enable_mask, PIC32_SET(sosc->enable_reg)); /* wait till warm-up period expires or ready-status is updated */ return readl_poll_timeout_atomic(sosc->status_reg, v, v & sosc->status_mask, 1, 100); } static void sosc_clk_disable(struct clk_hw *hw) { struct pic32_sec_osc *sosc = clkhw_to_sosc(hw); pic32_syskey_unlock(); writel(sosc->enable_mask, PIC32_CLR(sosc->enable_reg)); } static int sosc_clk_is_enabled(struct clk_hw *hw) { struct pic32_sec_osc *sosc = clkhw_to_sosc(hw); u32 enabled, ready; /* check enabled and ready status */ enabled = readl(sosc->enable_reg) & sosc->enable_mask; ready = readl(sosc->status_reg) & sosc->status_mask; return enabled && ready; } static unsigned long sosc_clk_calc_rate(struct clk_hw *hw, unsigned long parent_rate) { return clkhw_to_sosc(hw)->fixed_rate; } const struct clk_ops pic32_sosc_ops = { .enable = sosc_clk_enable, .disable = sosc_clk_disable, .is_enabled = sosc_clk_is_enabled, .recalc_rate = sosc_clk_calc_rate, }; struct clk *pic32_sosc_clk_register(const struct pic32_sec_osc_data *data, struct pic32_clk_common *core) { struct pic32_sec_osc *sosc; sosc = devm_kzalloc(core->dev, sizeof(*sosc), GFP_KERNEL); if (!sosc) return ERR_PTR(-ENOMEM); sosc->core = core; sosc->hw.init = &data->init_data; sosc->fixed_rate = data->fixed_rate; sosc->enable_mask = data->enable_mask; sosc->status_mask = data->status_mask; sosc->enable_reg = data->enable_reg + core->iobase; sosc->status_reg = data->status_reg + core->iobase; return devm_clk_register(core->dev, &sosc->hw); }