rockchip/clk/rk_clk_fract.c

/*-
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright 2019 Michal Meloun <mmel@FreeBSD.org>
 *
 * 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 <sys/cdefs.h>
#include <sys/param.h>
#include <sys/systm.h>
#include <sys/bus.h>

#include <dev/extres/clk/clk.h>

#include <arm64/rockchip/clk/rk_clk_fract.h>

#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))

#define	RK_CLK_FRACT_MASK_SHIFT	16

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);
static int rk_clk_fract_set_gate(struct clknode *clk, bool enable);

struct rk_clk_fract_sc {
	uint32_t	flags;
	uint32_t	offset;
	uint32_t	numerator;
	uint32_t	denominator;
	uint32_t	gate_offset;
	uint32_t	gate_shift;
};

static clknode_method_t rk_clk_fract_methods[] = {
	/* Device interface */
	CLKNODEMETHOD(clknode_init,		rk_clk_fract_init),
	CLKNODEMETHOD(clknode_set_gate,		rk_clk_fract_set_gate),
	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, &reg);
	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_gate(struct clknode *clk, bool enable)
{
	struct rk_clk_fract_sc *sc;
	uint32_t val = 0;

	sc = clknode_get_softc(clk);

	if ((sc->flags & RK_CLK_FRACT_HAVE_GATE) == 0)
		return (0);

	RD4(clk, sc->gate_offset, &val);

	val = 0;
	if (!enable)
		val |= 1 << sc->gate_shift;
	val |= (1 << sc->gate_shift) << RK_CLK_FRACT_MASK_SHIFT;
	DEVICE_LOCK(clk);
	WR4(clk, sc->gate_offset, val);
	DEVICE_UNLOCK(clk);

	return (0);
}

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;
	sc->gate_offset = clkdef->gate_offset;
	sc->gate_shift = clkdef->gate_shift;

	clknode_register(clkdom, clk);
	return (0);
}