rk_clk_fract.c

Version: - FREEBSD - FREEBSD-13-STABLE - FREEBSD-13-0 - FREEBSD-12-STABLE - FREEBSD-12-0 - FREEBSD-11-STABLE - FREEBSD-11-0 - FREEBSD-10-STABLE - FREEBSD-10-0 - FREEBSD-9-STABLE - FREEBSD-9-0 - FREEBSD-8-STABLE - FREEBSD-8-0 - FREEBSD-7-STABLE - FREEBSD-7-0 - FREEBSD-6-STABLE - FREEBSD-6-0 - FREEBSD-5-STABLE - FREEBSD-5-0 - FREEBSD-4-STABLE - FREEBSD-3-STABLE - FREEBSD22 - l41 - OPENBSD - linux-2.6 - MK84 - PLAN9 - xnu-8792
SearchContext: - none - 3 - 10

    1 /*-
    2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
    3  *
    4  * Copyright 2019 Michal Meloun <mmel@FreeBSD.org>
    5  *
    6  * Redistribution and use in source and binary forms, with or without
    7  * modification, are permitted provided that the following conditions
    8  * are met:
    9  * 1. Redistributions of source code must retain the above copyright
   10  *    notice, this list of conditions and the following disclaimer.
   11  * 2. Redistributions in binary form must reproduce the above copyright
   12  *    notice, this list of conditions and the following disclaimer in the
   13  *    documentation and/or other materials provided with the distribution.
   14  *
   15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
   16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   18  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
   19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
   20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
   21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
   22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
   24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
   25  * SUCH DAMAGE.
   26  */
   27 
   28 #include <sys/cdefs.h>
   29 __FBSDID("$FreeBSD$");
   30 
   31 #include <sys/param.h>
   32 #include <sys/systm.h>
   33 #include <sys/bus.h>
   34 
   35 #include <dev/extres/clk/clk.h>
   36 
   37 #include <arm64/rockchip/clk/rk_clk_fract.h>
   38 
   39 #include "clkdev_if.h"
   40 
   41 #define WR4(_clk, off, val)                                             \
   42         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
   43 #define RD4(_clk, off, val)                                             \
   44         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
   45 #define MD4(_clk, off, clr, set )                                       \
   46         CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set)
   47 #define DEVICE_LOCK(_clk)                                               \
   48         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
   49 #define DEVICE_UNLOCK(_clk)                                             \
   50         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
   51 
   52 #define RK_CLK_FRACT_MASK_SHIFT 16
   53 
   54 static int rk_clk_fract_init(struct clknode *clk, device_t dev);
   55 static int rk_clk_fract_recalc(struct clknode *clk, uint64_t *req);
   56 static int rk_clk_fract_set_freq(struct clknode *clknode, uint64_t fin,
   57     uint64_t *fout, int flag, int *stop);
   58 static int rk_clk_fract_set_gate(struct clknode *clk, bool enable);
   59 
   60 struct rk_clk_fract_sc {
   61         uint32_t        flags;
   62         uint32_t        offset;
   63         uint32_t        numerator;
   64         uint32_t        denominator;
   65         uint32_t        gate_offset;
   66         uint32_t        gate_shift;
   67 };
   68 
   69 static clknode_method_t rk_clk_fract_methods[] = {
   70         /* Device interface */
   71         CLKNODEMETHOD(clknode_init,             rk_clk_fract_init),
   72         CLKNODEMETHOD(clknode_set_gate,         rk_clk_fract_set_gate),
   73         CLKNODEMETHOD(clknode_recalc_freq,      rk_clk_fract_recalc),
   74         CLKNODEMETHOD(clknode_set_freq,         rk_clk_fract_set_freq),
   75         CLKNODEMETHOD_END
   76 };
   77 DEFINE_CLASS_1(rk_clk_fract, rk_clk_fract_class, rk_clk_fract_methods,
   78    sizeof(struct rk_clk_fract_sc), clknode_class);
   79 
   80 /*
   81  * Compute best rational approximation of input fraction
   82  * for fixed sized fractional divider registers.
   83  * http://en.wikipedia.org/wiki/Continued_fraction
   84  *
   85  * - n_input, d_input   Given input fraction
   86  * - n_max, d_max       Maximum vaues of divider registers
   87  * - n_out, d_out       Computed approximation
   88  */
   89 
   90 static void
   91 clk_compute_fract_div(
   92         uint64_t n_input, uint64_t d_input,
   93         uint64_t n_max, uint64_t d_max,
   94         uint64_t *n_out, uint64_t *d_out)
   95 {
   96         uint64_t n_prev, d_prev;        /* previous convergents */
   97         uint64_t n_cur, d_cur;          /* current  convergents */
   98         uint64_t n_rem, d_rem;          /* fraction remainder */
   99         uint64_t tmp, fact;
  100 
  101         /* Initialize fraction reminder */
  102         n_rem = n_input;
  103         d_rem = d_input;
  104 
  105         /* Init convergents to 0/1 and 1/0 */
  106         n_prev = 0;
  107         d_prev = 1;
  108         n_cur = 1;
  109         d_cur = 0;
  110 
  111         while (d_rem != 0 && n_cur < n_max && d_cur < d_max) {
  112                 /* Factor for this step. */
  113                 fact = n_rem / d_rem;
  114 
  115                 /* Adjust fraction reminder */
  116                 tmp = d_rem;
  117                 d_rem = n_rem % d_rem;
  118                 n_rem = tmp;
  119 
  120                 /* Compute new nominator and save last one */
  121                 tmp = n_prev + fact * n_cur;
  122                 n_prev = n_cur;
  123                 n_cur = tmp;
  124 
  125                 /* Compute new denominator and save last one */
  126                 tmp = d_prev + fact * d_cur;
  127                 d_prev = d_cur;
  128                 d_cur = tmp;
  129         }
  130 
  131         if (n_cur > n_max || d_cur > d_max) {
  132                 *n_out = n_prev;
  133                 *d_out = d_prev;
  134         } else {
  135                 *n_out = n_cur;
  136                 *d_out = d_cur;
  137         }
  138 }
  139 
  140 static int
  141 rk_clk_fract_init(struct clknode *clk, device_t dev)
  142 {
  143         uint32_t reg;
  144         struct rk_clk_fract_sc *sc;
  145 
  146         sc = clknode_get_softc(clk);
  147         DEVICE_LOCK(clk);
  148         RD4(clk, sc->offset, &reg);
  149         DEVICE_UNLOCK(clk);
  150 
  151         sc->numerator  = (reg >> 16) & 0xFFFF;
  152         sc->denominator  = reg & 0xFFFF;
  153         if (sc->denominator == 0)
  154                 sc->denominator = 1;
  155         clknode_init_parent_idx(clk, 0);
  156 
  157         return(0);
  158 }
  159 
  160 static int
  161 rk_clk_fract_set_gate(struct clknode *clk, bool enable)
  162 {
  163         struct rk_clk_fract_sc *sc;
  164         uint32_t val = 0;
  165 
  166         sc = clknode_get_softc(clk);
  167 
  168         if ((sc->flags & RK_CLK_FRACT_HAVE_GATE) == 0)
  169                 return (0);
  170 
  171         RD4(clk, sc->gate_offset, &val);
  172 
  173         val = 0;
  174         if (!enable)
  175                 val |= 1 << sc->gate_shift;
  176         val |= (1 << sc->gate_shift) << RK_CLK_FRACT_MASK_SHIFT;
  177         DEVICE_LOCK(clk);
  178         WR4(clk, sc->gate_offset, val);
  179         DEVICE_UNLOCK(clk);
  180 
  181         return (0);
  182 }
  183 
  184 static int
  185 rk_clk_fract_recalc(struct clknode *clk, uint64_t *freq)
  186 {
  187         struct rk_clk_fract_sc *sc;
  188 
  189         sc = clknode_get_softc(clk);
  190         if (sc->denominator == 0) {
  191                 printf("%s: %s denominator is zero!\n", clknode_get_name(clk),
  192                 __func__);
  193                 *freq = 0;
  194                 return(EINVAL);
  195         }
  196 
  197         *freq *= sc->numerator;
  198         *freq /= sc->denominator;
  199 
  200         return (0);
  201 }
  202 
  203 static int
  204 rk_clk_fract_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
  205     int flags, int *stop)
  206 {
  207         struct rk_clk_fract_sc *sc;
  208         uint64_t div_n, div_d, _fout;
  209 
  210         sc = clknode_get_softc(clk);
  211 
  212         clk_compute_fract_div(*fout, fin, 0xFFFF, 0xFFFF, &div_n, &div_d);
  213         _fout = fin * div_n;
  214         _fout /= div_d;
  215 
  216         /* Rounding. */
  217         if ((flags & CLK_SET_ROUND_UP) && (_fout < *fout)) {
  218                 if (div_n > div_d && div_d > 1)
  219                         div_n++;
  220                 else
  221                         div_d--;
  222         } else if ((flags & CLK_SET_ROUND_DOWN) && (_fout > *fout)) {
  223                 if (div_n > div_d && div_n > 1)
  224                         div_n--;
  225                 else
  226                         div_d++;
  227         }
  228 
  229         /* Check range after rounding */
  230         if (div_n > 0xFFFF || div_d > 0xFFFF)
  231                 return (ERANGE);
  232 
  233         if (div_d == 0) {
  234                 printf("%s: %s divider is zero!\n",
  235                      clknode_get_name(clk), __func__);
  236                 return(EINVAL);
  237         }
  238         /* Recompute final output frequency */
  239         _fout = fin * div_n;
  240         _fout /= div_d;
  241 
  242         *stop = 1;
  243 
  244         if ((flags & CLK_SET_DRYRUN) == 0) {
  245                 if (*stop != 0 &&
  246                     (flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0 &&
  247                     *fout != _fout)
  248                         return (ERANGE);
  249 
  250                 sc->numerator  = (uint32_t)div_n;
  251                 sc->denominator = (uint32_t)div_d;
  252 
  253                 DEVICE_LOCK(clk);
  254                 WR4(clk, sc->offset, sc->numerator << 16 | sc->denominator);
  255                 DEVICE_UNLOCK(clk);
  256         }
  257 
  258         *fout = _fout;
  259         return (0);
  260 }
  261 
  262 int
  263 rk_clk_fract_register(struct clkdom *clkdom, struct rk_clk_fract_def *clkdef)
  264 {
  265         struct clknode *clk;
  266         struct rk_clk_fract_sc *sc;
  267 
  268         clk = clknode_create(clkdom, &rk_clk_fract_class, &clkdef->clkdef);
  269         if (clk == NULL)
  270                 return (1);
  271 
  272         sc = clknode_get_softc(clk);
  273         sc->flags = clkdef->flags;
  274         sc->offset = clkdef->offset;
  275         sc->gate_offset = clkdef->gate_offset;
  276         sc->gate_shift = clkdef->gate_shift;
  277 
  278         clknode_register(clkdom, clk);
  279         return (0);
  280 }

Cache object: cc14c4802508e4ef6d0f84638b1f236a

[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]

This page is part of the FreeBSD/Linux Linux Kernel Cross-Reference, and was automatically generated using a modified version of the LXR engine.

FreeBSD/Linux Kernel Cross Reference sys/arm64/rockchip/clk/rk_clk_fract.c

FreeBSD/Linux Kernel Cross Reference
sys/arm64/rockchip/clk/rk_clk_fract.c