Index: head/sys/arm64/rockchip/clk/rk_clk_fract.h =================================================================== --- head/sys/arm64/rockchip/clk/rk_clk_fract.h +++ head/sys/arm64/rockchip/clk/rk_clk_fract.h @@ -0,0 +1,44 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright 2019 Michal Meloun + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $FreeBSD$ + */ + +#ifndef _RK_CLK_FRACT_H_ +#define _RK_CLK_FRACT_H_ + +#include + +struct rk_clk_fract_def { + struct clknode_init_def clkdef; + uint32_t offset; + uint32_t flags; +}; + +int rk_clk_fract_register(struct clkdom *clkdom, + struct rk_clk_fract_def *clkdef); + +#endif /* _RK_CLK_FRACT_H_ */ Index: head/sys/arm64/rockchip/clk/rk_clk_fract.c =================================================================== --- head/sys/arm64/rockchip/clk/rk_clk_fract.c +++ head/sys/arm64/rockchip/clk/rk_clk_fract.c @@ -0,0 +1,249 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright 2019 Michal Meloun + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ + +#include +__FBSDID("$FreeBSD$"); + +#include +#include +#include + +#include + +#include + +#include "clkdev_if.h" + +#define WR4(_clk, off, val) \ + CLKDEV_WRITE_4(clknode_get_device(_clk), off, val) +#define RD4(_clk, off, val) \ + CLKDEV_READ_4(clknode_get_device(_clk), off, val) +#define MD4(_clk, off, clr, set ) \ + CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set) +#define DEVICE_LOCK(_clk) \ + CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) +#define DEVICE_UNLOCK(_clk) \ + CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) + +static int rk_clk_fract_init(struct clknode *clk, device_t dev); +static int rk_clk_fract_recalc(struct clknode *clk, uint64_t *req); +static int rk_clk_fract_set_freq(struct clknode *clknode, uint64_t fin, + uint64_t *fout, int flag, int *stop); + +struct rk_clk_fract_sc { + uint32_t flags; + uint32_t offset; + uint32_t numerator; + uint32_t denominator; +}; + +static clknode_method_t rk_clk_fract_methods[] = { + /* Device interface */ + CLKNODEMETHOD(clknode_init, rk_clk_fract_init), + CLKNODEMETHOD(clknode_recalc_freq, rk_clk_fract_recalc), + CLKNODEMETHOD(clknode_set_freq, rk_clk_fract_set_freq), + CLKNODEMETHOD_END +}; +DEFINE_CLASS_1(rk_clk_fract, rk_clk_fract_class, rk_clk_fract_methods, + sizeof(struct rk_clk_fract_sc), clknode_class); + + +/* + * Compute best rational approximation of input fraction + * for fixed sized fractional divider registers. + * http://en.wikipedia.org/wiki/Continued_fraction + * + * - n_input, d_input Given input fraction + * - n_max, d_max Maximum vaues of divider registers + * - n_out, d_out Computed approximation + */ + +static void +clk_compute_fract_div( + uint64_t n_input, uint64_t d_input, + uint64_t n_max, uint64_t d_max, + uint64_t *n_out, uint64_t *d_out) +{ + uint64_t n_prev, d_prev; /* previous convergents */ + uint64_t n_cur, d_cur; /* current convergents */ + uint64_t n_rem, d_rem; /* fraction remainder */ + uint64_t tmp, fact; + + /* Initialize fraction reminder */ + n_rem = n_input; + d_rem = d_input; + + /* Init convergents to 0/1 and 1/0 */ + n_prev = 0; + d_prev = 1; + n_cur = 1; + d_cur = 0; + + while (d_rem != 0 && n_cur < n_max && d_cur < d_max) { + /* Factor for this step. */ + fact = n_rem / d_rem; + + /* Adjust fraction reminder */ + tmp = d_rem; + d_rem = n_rem % d_rem; + n_rem = tmp; + + /* Compute new nominator and save last one */ + tmp = n_prev + fact * n_cur; + n_prev = n_cur; + n_cur = tmp; + + /* Compute new denominator and save last one */ + tmp = d_prev + fact * d_cur; + d_prev = d_cur; + d_cur = tmp; + } + + if (n_cur > n_max || d_cur > d_max) { + *n_out = n_prev; + *d_out = d_prev; + } else { + *n_out = n_cur; + *d_out = d_cur; + } +} + +static int +rk_clk_fract_init(struct clknode *clk, device_t dev) +{ + uint32_t reg; + struct rk_clk_fract_sc *sc; + + sc = clknode_get_softc(clk); + DEVICE_LOCK(clk); + RD4(clk, sc->offset, ®); + DEVICE_UNLOCK(clk); + + sc->numerator = (reg >> 16) & 0xFFFF; + sc->denominator = reg & 0xFFFF; + clknode_init_parent_idx(clk, 0); + + return(0); +} +static int +rk_clk_fract_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout, + int flags, int *stop); +static int +rk_clk_fract_recalc(struct clknode *clk, uint64_t *freq) +{ + struct rk_clk_fract_sc *sc; + + sc = clknode_get_softc(clk); + if (sc->denominator == 0) { + printf("%s: %s denominator is zero!\n", clknode_get_name(clk), + __func__); + *freq = 0; + return(EINVAL); + } + + *freq *= sc->numerator; + *freq /= sc->denominator; + + return (0); +} + +static int +rk_clk_fract_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout, + int flags, int *stop) +{ + struct rk_clk_fract_sc *sc; + uint64_t div_n, div_d, _fout; + + sc = clknode_get_softc(clk); + + clk_compute_fract_div(*fout, fin, 0xFFFF, 0xFFFF, &div_n, &div_d); + _fout = fin * div_n; + _fout /= div_d; + + /* Rounding. */ + if ((flags & CLK_SET_ROUND_UP) && (_fout < *fout)) { + if (div_n > div_d && div_d > 1) + div_n++; + else + div_d--; + } else if ((flags & CLK_SET_ROUND_DOWN) && (_fout > *fout)) { + if (div_n > div_d && div_n > 1) + div_n--; + else + div_d++; + } + + /* Check range after rounding */ + if (div_n > 0xFFFF || div_d > 0xFFFF) + return (ERANGE); + + if (div_d == 0) { + printf("%s: %s divider is zero!\n", + clknode_get_name(clk), __func__); + return(EINVAL); + } + /* Recompute final output frequency */ + _fout = fin * div_n; + _fout /= div_d; + + *stop = 1; + + if ((flags & CLK_SET_DRYRUN) == 0) { + if (*stop != 0 && + (flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0 && + *fout != _fout) + return (ERANGE); + + sc->numerator = (uint32_t)div_n; + sc->denominator = (uint32_t)div_d; + + DEVICE_LOCK(clk); + WR4(clk, sc->offset, sc->numerator << 16 | sc->denominator); + DEVICE_UNLOCK(clk); + } + + *fout = _fout; + return (0); +} + +int +rk_clk_fract_register(struct clkdom *clkdom, struct rk_clk_fract_def *clkdef) +{ + struct clknode *clk; + struct rk_clk_fract_sc *sc; + + clk = clknode_create(clkdom, &rk_clk_fract_class, &clkdef->clkdef); + if (clk == NULL) + return (1); + + sc = clknode_get_softc(clk); + sc->flags = clkdef->flags; + sc->offset = clkdef->offset; + + clknode_register(clkdom, clk); + return (0); +} Index: head/sys/arm64/rockchip/clk/rk_cru.h =================================================================== --- head/sys/arm64/rockchip/clk/rk_cru.h +++ head/sys/arm64/rockchip/clk/rk_cru.h @@ -31,8 +31,15 @@ #ifndef __RK_CRU_H__ #define __RK_CRU_H__ +#include +#include +#include +#include +#include + #include #include +#include #include #include #include @@ -59,8 +66,11 @@ RK3328_CLK_PLL, RK3399_CLK_PLL, RK_CLK_COMPOSITE, + RK_CLK_FIXED, + RK_CLK_FRACT, RK_CLK_MUX, RK_CLK_ARMCLK, + RK_CLK_LINK, }; struct rk_clk { @@ -70,6 +80,9 @@ struct rk_clk_composite_def *composite; struct rk_clk_mux_def *mux; struct rk_clk_armclk_def *armclk; + struct clk_fixed_def *fixed; + struct rk_clk_fract_def *fract; + struct clk_link_def *link; } clk; }; Index: head/sys/arm64/rockchip/clk/rk_cru.c =================================================================== --- head/sys/arm64/rockchip/clk/rk_cru.c +++ head/sys/arm64/rockchip/clk/rk_cru.c @@ -52,6 +52,8 @@ #include #include +#include +#include #include #include @@ -251,12 +253,24 @@ rk_clk_armclk_register(sc->clkdom, sc->clks[i].clk.armclk); break; + case RK_CLK_FIXED: + clknode_fixed_register(sc->clkdom, + sc->clks[i].clk.fixed); + break; + case RK_CLK_FRACT: + rk_clk_fract_register(sc->clkdom, + sc->clks[i].clk.fract); + break; + case RK_CLK_LINK: + clknode_link_register(sc->clkdom, + sc->clks[i].clk.link); + break; default: device_printf(dev, "Unknown clock type\n"); return (ENXIO); - break; } } + if (sc->gates) rk_cru_register_gates(sc); Index: head/sys/conf/files.arm64 =================================================================== --- head/sys/conf/files.arm64 +++ head/sys/conf/files.arm64 @@ -309,6 +309,7 @@ arm64/rockchip/clk/rk_cru.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 arm64/rockchip/clk/rk_clk_armclk.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 arm64/rockchip/clk/rk_clk_composite.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 +arm64/rockchip/clk/rk_clk_fract.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 arm64/rockchip/clk/rk_clk_gate.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 arm64/rockchip/clk/rk_clk_mux.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399 arm64/rockchip/clk/rk_clk_pll.c optional fdt soc_rockchip_rk3328 | fdt soc_rockchip_rk3399