path: root/drivers/soc/xilinx/xlnx_vcu_clk.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Xilinx VCU clock driver
 *
 * Copyright (C) 2018 Xilinx, Inc.
 *
 * Rajan Vaja <rajan.vaja@xilinx.com>
 * Tejas Patel <tejas.patel@xilinx.com>
 */

#include <linux/clk.h>
#include <linux/clk-provider.h>
#include <linux/device.h>
#include <linux/errno.h>
#include <linux/io.h>
#include <linux/iopoll.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_address.h>
#include <linux/of_platform.h>
#include <linux/platform_device.h>
#include <linux/slab.h>
#include <soc/xilinx/xlnx_vcu.h>

/* vcu slcr registers, bitmask and shift */
#define VCU_PLL_CTRL			0x24
#define VCU_PLL_CTRL_RESET_MASK		BIT(0)
#define VCU_PLL_CTRL_RESET_SHIFT	0
#define VCU_PLL_CTRL_BYPASS_MASK	BIT(3)
#define VCU_PLL_CTRL_BYPASS_SHIFT	3
#define VCU_PLL_CTRL_FBDIV_MASK		0x7f
#define VCU_PLL_CTRL_FBDIV_SHIFT	8
#define VCU_PLL_CTRL_POR_IN_MASK	BIT(1)
#define VCU_PLL_CTRL_POR_IN_SHIFT	1
#define VCU_PLL_CTRL_PWR_POR_MASK	BIT(2)
#define VCU_PLL_CTRL_PWR_POR_SHIFT	2
#define VCU_PLL_CTRL_CLKOUTDIV_MASK	0x03
#define VCU_PLL_CTRL_CLKOUTDIV_SHIFT	16
#define VCU_PLL_CTRL_DEFAULT		0

#define VCU_PLL_CFG			0x28
#define VCU_PLL_CFG_RES_MASK		0x0f
#define VCU_PLL_CFG_RES_SHIFT		0
#define VCU_PLL_CFG_CP_MASK		0x0f
#define VCU_PLL_CFG_CP_SHIFT		5
#define VCU_PLL_CFG_LFHF_MASK		0x03
#define VCU_PLL_CFG_LFHF_SHIFT		10
#define VCU_PLL_CFG_LOCK_CNT_MASK	0x03ff
#define VCU_PLL_CFG_LOCK_CNT_SHIFT	13
#define VCU_PLL_CFG_LOCK_DLY_MASK	0x7f
#define VCU_PLL_CFG_LOCK_DLY_SHIFT	25
#define VCU_ENC_CORE_CTRL		0x30
#define VCU_ENC_MCU_CTRL		0x34
#define VCU_ENC_MCU_CTRL_GATE_BIT	BIT(12)
#define VCU_DEC_CORE_CTRL		0x38
#define VCU_DEC_MCU_CTRL		0x3c
#define VCU_PLL_DIVISOR_MASK		0x3f
#define VCU_PLL_DIVISOR_SHIFT		4
#define VCU_SRCSEL_MASK			0x01
#define VCU_SRCSEL_SHIFT		0
#define VCU_SRCSEL_PLL			1

#define VCU_PLL_STATUS			0x60
#define VCU_PLL_STATUS_LOCK_STATUS_MASK	0x01
#define VCU_PLL_LOCK_TIMEOUT		2000000

#define PLL_FBDIV_MIN			25
#define PLL_FBDIV_MAX			125

#define MHZ				1000000
#define FVCO_MIN			(1500U * MHZ)
#define FVCO_MAX			(3000U * MHZ)
#define DIVISOR_MIN			0
#define DIVISOR_MAX			63
#define FRAC				100
#define LIMIT				(10 * MHZ)

#define FRAC_OFFSET			0x8
#define PLLFCFG_FRAC_EN			BIT(31)
#define FRAC_DIV			0x10000 /* 2^16 */

#define to_vcu_pll(_hw)	container_of(_hw, struct vcu_pll, hw)
#define div_mask(width)	((1 << (width)) - 1)

enum pll_mode {
	PLL_MODE_INT,
	PLL_MODE_FRAC,
};

enum vcu_clks {
	vcu_pll_half, vcu_core_enc, vcu_core_dec,
	mcu_core_enc, mcu_core_dec, clk_max
};

/**
 * struct xvcu_pll_cfg - Helper data
 * @fbdiv: The integer portion of the feedback divider to the PLL
 * @cp:	PLL charge pump control
 * @res: PLL loop filter resistor control
 * @lfhf: PLL loop filter high frequency capacitor control
 * @lock_dly: Lock circuit configuration settings for lock windowsize
 * @lock_cnt: Lock circuit counter setting
 */
struct xvcu_pll_cfg {
	u32 fbdiv;
	u32 cp;
	u32 res;
	u32 lfhf;
	u32 lock_dly;
	u32 lock_cnt;
};

/**
 * struct vcu_pll - VCU PLL control/status data
 * @hw:	Clock hardware
 * @pll_ctrl: PLL control register address
 * @pll_status: PLL status register address
 * @pll_cfg: PLL config register address
 * @lockbit: PLL lock status bit
 */
struct vcu_pll {
	struct clk_hw	hw;
	void __iomem	*pll_ctrl;
	void __iomem	*pll_status;
	void __iomem	*pll_cfg;
	u8		lockbit;
};

static struct clk_hw_onecell_data *vcu_clk_data;
static const char * const vcu_mux_parents[] = {
	"dummy_name",
	"vcu_pll_half"
};

static DEFINE_SPINLOCK(mcu_enc_lock);
static DEFINE_SPINLOCK(mcu_dec_lock);
static DEFINE_SPINLOCK(core_enc_lock);
static DEFINE_SPINLOCK(core_dec_lock);

static const struct xvcu_pll_cfg xvcu_pll_cfg[] = {
	{ 25, 3, 10, 3, 63, 1000 },
	{ 26, 3, 10, 3, 63, 1000 },
	{ 27, 4, 6, 3, 63, 1000 },
	{ 28, 4, 6, 3, 63, 1000 },
	{ 29, 4, 6, 3, 63, 1000 },
	{ 30, 4, 6, 3, 63, 1000 },
	{ 31, 6, 1, 3, 63, 1000 },
	{ 32, 6, 1, 3, 63, 1000 },
	{ 33, 4, 10, 3, 63, 1000 },
	{ 34, 5, 6, 3, 63, 1000 },
	{ 35, 5, 6, 3, 63, 1000 },
	{ 36, 5, 6, 3, 63, 1000 },
	{ 37, 5, 6, 3, 63, 1000 },
	{ 38, 5, 6, 3, 63, 975 },
	{ 39, 3, 12, 3, 63, 950 },
	{ 40, 3, 12, 3, 63, 925 },
	{ 41, 3, 12, 3, 63, 900 },
	{ 42, 3, 12, 3, 63, 875 },
	{ 43, 3, 12, 3, 63, 850 },
	{ 44, 3, 12, 3, 63, 850 },
	{ 45, 3, 12, 3, 63, 825 },
	{ 46, 3, 12, 3, 63, 800 },
	{ 47, 3, 12, 3, 63, 775 },
	{ 48, 3, 12, 3, 63, 775 },
	{ 49, 3, 12, 3, 63, 750 },
	{ 50, 3, 12, 3, 63, 750 },
	{ 51, 3, 2, 3, 63, 725 },
	{ 52, 3, 2, 3, 63, 700 },
	{ 53, 3, 2, 3, 63, 700 },
	{ 54, 3, 2, 3, 63, 675 },
	{ 55, 3, 2, 3, 63, 675 },
	{ 56, 3, 2, 3, 63, 650 },
	{ 57, 3, 2, 3, 63, 650 },
	{ 58, 3, 2, 3, 63, 625 },
	{ 59, 3, 2, 3, 63, 625 },
	{ 60, 3, 2, 3, 63, 625 },
	{ 61, 3, 2, 3, 63, 600 },
	{ 62, 3, 2, 3, 63, 600 },
	{ 63, 3, 2, 3, 63, 600 },
	{ 64, 3, 2, 3, 63, 600 },
	{ 65, 3, 2, 3, 63, 600 },
	{ 66, 3, 2, 3, 63, 600 },
	{ 67, 3, 2, 3, 63, 600 },
	{ 68, 3, 2, 3, 63, 600 },
	{ 69, 3, 2, 3, 63, 600 },
	{ 70, 3, 2, 3, 63, 600 },
	{ 71, 3, 2, 3, 63, 600 },
	{ 72, 3, 2, 3, 63, 600 },
	{ 73, 3, 2, 3, 63, 600 },
	{ 74, 3, 2, 3, 63, 600 },
	{ 75, 3, 2, 3, 63, 600 },
	{ 76, 3, 2, 3, 63, 600 },
	{ 77, 3, 2, 3, 63, 600 },
	{ 78, 3, 2, 3, 63, 600 },
	{ 79, 3, 2, 3, 63, 600 },
	{ 80, 3, 2, 3, 63, 600 },
	{ 81, 3, 2, 3, 63, 600 },
	{ 82, 3, 2, 3, 63, 600 },
	{ 83, 4, 2, 3, 63, 600 },
	{ 84, 4, 2, 3, 63, 600 },
	{ 85, 4, 2, 3, 63, 600 },
	{ 86, 4, 2, 3, 63, 600 },
	{ 87, 4, 2, 3, 63, 600 },
	{ 88, 4, 2, 3, 63, 600 },
	{ 89, 4, 2, 3, 63, 600 },
	{ 90, 4, 2, 3, 63, 600 },
	{ 91, 4, 2, 3, 63, 600 },
	{ 92, 4, 2, 3, 63, 600 },
	{ 93, 4, 2, 3, 63, 600 },
	{ 94, 4, 2, 3, 63, 600 },
	{ 95, 4, 2, 3, 63, 600 },
	{ 96, 4, 2, 3, 63, 600 },
	{ 97, 4, 2, 3, 63, 600 },
	{ 98, 4, 2, 3, 63, 600 },
	{ 99, 4, 2, 3, 63, 600 },
	{ 100, 4, 2, 3, 63, 600 },
	{ 101, 4, 2, 3, 63, 600 },
	{ 102, 4, 2, 3, 63, 600 },
	{ 103, 5, 2, 3, 63, 600 },
	{ 104, 5, 2, 3, 63, 600 },
	{ 105, 5, 2, 3, 63, 600 },
	{ 106, 5, 2, 3, 63, 600 },
	{ 107, 3, 4, 3, 63, 600 },
	{ 108, 3, 4, 3, 63, 600 },
	{ 109, 3, 4, 3, 63, 600 },
	{ 110, 3, 4, 3, 63, 600 },
	{ 111, 3, 4, 3, 63, 600 },
	{ 112, 3, 4, 3, 63, 600 },
	{ 113, 3, 4, 3, 63, 600 },
	{ 114, 3, 4, 3, 63, 600 },
	{ 115, 3, 4, 3, 63, 600 },
	{ 116, 3, 4, 3, 63, 600 },
	{ 117, 3, 4, 3, 63, 600 },
	{ 118, 3, 4, 3, 63, 600 },
	{ 119, 3, 4, 3, 63, 600 },
	{ 120, 3, 4, 3, 63, 600 },
	{ 121, 3, 4, 3, 63, 600 },
	{ 122, 3, 4, 3, 63, 600 },
	{ 123, 3, 4, 3, 63, 600 },
	{ 124, 3, 4, 3, 63, 600 },
	{ 125, 3, 4, 3, 63, 600 },
};

static int xvcu_divider_get_val(unsigned long rate, unsigned long parent_rate,
				const struct clk_div_table *table, u8 width,
				unsigned long flags)
{
	unsigned int div;

	if (flags & CLK_DIVIDER_ROUND_CLOSEST)
		div = DIV_ROUND_CLOSEST_ULL((u64)parent_rate, rate);
	else
		div = DIV_ROUND_UP_ULL((u64)parent_rate, rate);

	return min_t(unsigned int, div, div_mask(width));
}

static unsigned long xvcu_divider_recalc_rate(struct clk_hw *hw,
					      unsigned long parent_rate)
{
	struct clk_divider *divider = to_clk_divider(hw);
	unsigned int val;

	val = readl(divider->reg) >> divider->shift;
	val &= div_mask(divider->width);

	return divider_recalc_rate(hw, parent_rate, val, divider->table,
				   divider->flags, divider->width);
}

static long xvcu_divider_round_rate(struct clk_hw *hw, unsigned long rate,
				    unsigned long *prate)
{
	struct clk_divider *divider = to_clk_divider(hw);
	int bestdiv;

	bestdiv = xvcu_divider_get_val(rate, *prate, divider->table,
				       divider->width, divider->flags);

	*prate = rate * bestdiv;

	return rate;
}

static int xvcu_divider_set_rate(struct clk_hw *hw, unsigned long rate,
				 unsigned long parent_rate)
{
	struct clk_divider *divider = to_clk_divider(hw);
	int value;
	u32 val;

	value = xvcu_divider_get_val(rate, parent_rate, divider->table,
				     divider->width, divider->flags);
	if (value < 0)
		return value;

	val = readl(divider->reg);
	val &= ~(div_mask(divider->width) << divider->shift);
	val |= (u32)value << divider->shift;
	writel(val, divider->reg);

	return 0;
}

static const struct clk_ops xvcu_divider_ops = {
	.recalc_rate = xvcu_divider_recalc_rate,
	.round_rate = xvcu_divider_round_rate,
	.set_rate = xvcu_divider_set_rate,
};

/**
 * xvcu_register_divider - Register custom divider hardware
 * @dev:		VCU clock device
 * @name:		Divider name
 * @parent_name:	Divider parent name
 * @flags:		Clock flags
 * @reg:		Divider register base address
 * @shift:		Divider bits shift
 * @width:		Divider bits width
 * @clk_divider_flags:	Divider specific flags
 * @lock:		Shared register lock
 *
 * Register custom divider hardware to CCF.
 *
 * Return: Clock hardware for generated clock
 */
static struct clk_hw *xvcu_register_divider(struct device *dev,
					    const char *name,
					    const char *parent_name,
					    unsigned long flags,
					    void __iomem *reg, u8 shift,
					    u8 width, u8 clk_divider_flags,
					    spinlock_t *lock)
{
	struct clk_divider *div;
	struct clk_hw *hw;
	struct clk_init_data init;
	int ret;

	/* allocate the divider */
	div = kzalloc(sizeof(*div), GFP_KERNEL);
	if (!div)
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &xvcu_divider_ops;
	init.flags = flags;
	init.parent_names = (parent_name ? &parent_name : NULL);
	init.num_parents = (parent_name ? 1 : 0);

	/* struct clk_divider assignments */
	div->reg = reg;
	div->shift = shift;
	div->width = width;
	div->flags = clk_divider_flags;
	div->lock = lock;
	div->hw.init = &init;

	/* register the clock */
	hw = &div->hw;
	ret = clk_hw_register(dev, hw);
	if (ret) {
		kfree(div);
		hw = ERR_PTR(ret);
	}

	return hw;
}

/**
 * xvcu_pll_bypass_ctrl - Enable/Disable PLL bypass mode
 * @pll:	PLL data
 * @enable:	Enable/Disable flag
 *
 * Enable/Disable PLL bypass mode:
 *	0 - Disable
 *	1 - Enable
 */
static void xvcu_pll_bypass_ctrl(struct vcu_pll *pll, bool enable)
{
	u32 reg;

	reg = readl(pll->pll_ctrl);
	if (enable)
		reg |= VCU_PLL_CTRL_BYPASS_MASK;
	else
		reg &= ~VCU_PLL_CTRL_BYPASS_MASK;
	writel(reg, pll->pll_ctrl);
}

/**
 * xvcu_pll_config - Configure PLL based on FBDIV value
 * @pll:	PLL data
 *
 * PLL needs to be configured before taking out of reset. Configuration
 * data depends on the value of FBDIV for proper PLL locking.
 */
static void xvcu_pll_config(struct vcu_pll *pll)
{
	unsigned int fbdiv, reg;
	int i;

	reg = readl(pll->pll_ctrl);
	fbdiv = (reg >> VCU_PLL_CTRL_FBDIV_SHIFT) & VCU_PLL_CTRL_FBDIV_MASK;

	for (i = ARRAY_SIZE(xvcu_pll_cfg) - 1; i >= 0; i--) {
		if (fbdiv != xvcu_pll_cfg[i].fbdiv)
			continue;

		/* Set RES, CP, LFHF, LOCK_CNT and LOCK_DLY cfg values */
		reg = (xvcu_pll_cfg[i].res << VCU_PLL_CFG_RES_SHIFT) |
		      (xvcu_pll_cfg[i].cp << VCU_PLL_CFG_CP_SHIFT) |
		      (xvcu_pll_cfg[i].lfhf << VCU_PLL_CFG_LFHF_SHIFT) |
		      (xvcu_pll_cfg[i].lock_cnt << VCU_PLL_CFG_LOCK_CNT_SHIFT) |
		      (xvcu_pll_cfg[i].lock_dly << VCU_PLL_CFG_LOCK_DLY_SHIFT);
		writel(reg, pll->pll_cfg);
	}
}

/**
 * xvcu_pll_enable_disable - Enable/Disable PLL
 * @pll:	PLL data
 * @enable:	Enable/Disable flag
 *
 * Enable/Disable PLL based on request:
 *	0 - Disable
 *	1 - Enable
 */
static void xvcu_pll_enable_disable(struct vcu_pll *pll, bool enable)
{
	u32 reg;

	reg = readl(pll->pll_ctrl);
	if (enable)
		reg &= ~(VCU_PLL_CTRL_RESET_MASK | VCU_PLL_CTRL_POR_IN_MASK |
				VCU_PLL_CTRL_PWR_POR_MASK);
	else
		reg |= (VCU_PLL_CTRL_RESET_MASK | VCU_PLL_CTRL_POR_IN_MASK |
				VCU_PLL_CTRL_PWR_POR_MASK);
	writel(reg, pll->pll_ctrl);
}

/**
 * xvcu_pll_is_enabled - Check if PLL is enabled or not
 * @hw:		Clock hardware
 *
 * Check if PLL is enabled or not. PLL enabled means PLL is not in
 * reset state.
 *
 * Return: PLL status (0 - Disabled, 1 - Enabled)
 */
static int xvcu_pll_is_enabled(struct clk_hw *hw)
{
	struct vcu_pll *pll = to_vcu_pll(hw);
	u32 reg;

	reg = readl(pll->pll_ctrl);

	return !(reg & (VCU_PLL_CTRL_RESET_MASK | VCU_PLL_CTRL_POR_IN_MASK |
		 VCU_PLL_CTRL_PWR_POR_MASK));
}

/**
 * xvcu_pll_enable - Enable PLL
 * @hw:		Clock hardware
 *
 * Enable PLL if it is not enabled. Configure PLL, enable and wait for
 * the PLL lock. Put PLL into bypass state during PLL configuration.
 *
 * Return: 0 on success else error code
 */
static int xvcu_pll_enable(struct clk_hw *hw)
{
	struct vcu_pll *pll = to_vcu_pll(hw);
	u32 reg;
	int ret;

	if (xvcu_pll_is_enabled(hw))
		return 0;

	pr_info("VCU PLL: enable\n");

	xvcu_pll_bypass_ctrl(pll, 1);

	xvcu_pll_config(pll);

	xvcu_pll_enable_disable(pll, 1);

	ret = readl_poll_timeout_atomic(pll->pll_status, reg,
					reg & VCU_PLL_STATUS_LOCK_STATUS_MASK,
					1, VCU_PLL_LOCK_TIMEOUT);
	if (ret) {
		pr_err("VCU PLL is not locked\n");
		return ret;
	}

	xvcu_pll_bypass_ctrl(pll, 0);

	return ret;
}

/**
 * xvcu_pll_disable - Disable PLL
 * @hw:		Clock hardware
 *
 * Disable PLL if it is enabled.
 *
 * Return: 0 on success else error code
 */
static void xvcu_pll_disable(struct clk_hw *hw)
{
	struct vcu_pll *pll = to_vcu_pll(hw);

	if (!xvcu_pll_is_enabled(hw))
		return;

	pr_info("PLL: shutdown\n");
	xvcu_pll_enable_disable(pll, 0);
}

/**
 * xvcu_pll_frac_get_mode - Get PLL fraction mode
 * @hw:		Clock hardware
 *
 * Check if PLL is configured for integer mode or fraction mode.
 *
 * Return: PLL mode:
 *	PLL_MODE_FRAC - Fraction mode
 *	PLL_MODE_INT - Integer mode
 */
static inline enum pll_mode xvcu_pll_frac_get_mode(struct clk_hw *hw)
{
	struct vcu_pll *clk = to_vcu_pll(hw);
	u32 reg;

	reg = readl(clk->pll_ctrl + FRAC_OFFSET);

	reg = reg & PLLFCFG_FRAC_EN;
	return reg ? PLL_MODE_FRAC : PLL_MODE_INT;
}

/**
 * xvcu_pll_frac_set_mode - Set PLL fraction mode
 * @hw:		Clock hardware
 * @on:		Enable/Disable flag
 *
 * Configure PLL for integer mode or fraction mode.
 *	1 - Fraction mode
 *	0 - Integer mode
 */
static inline void xvcu_pll_frac_set_mode(struct clk_hw *hw, bool on)
{
	struct vcu_pll *clk = to_vcu_pll(hw);
	u32 reg = 0;

	if (on)
		reg = PLLFCFG_FRAC_EN;

	reg = readl(clk->pll_ctrl + FRAC_OFFSET);
	reg |= PLLFCFG_FRAC_EN;
	writel(reg, (clk->pll_ctrl + FRAC_OFFSET));
}

static long vcu_pll_round_rate(struct clk_hw *hw, unsigned long rate,
			       unsigned long *prate)
{
	u32 fbdiv;
	long rate_div, f;

	/* Enable the fractional mode if needed */
	rate_div = (rate * FRAC_DIV) / *prate;
	f = rate_div % FRAC_DIV;
	xvcu_pll_frac_set_mode(hw, !!f);

	if (xvcu_pll_frac_get_mode(hw) == PLL_MODE_FRAC) {
		if (rate > FVCO_MAX) {
			fbdiv = rate / FVCO_MAX;
			rate = rate / (fbdiv + 1);
		}
		if (rate < FVCO_MIN) {
			fbdiv = DIV_ROUND_UP(FVCO_MIN, rate);
			rate = rate * fbdiv;
		}
		return rate;
	}

	fbdiv = DIV_ROUND_CLOSEST(rate, *prate);
	fbdiv = clamp_t(u32, fbdiv, PLL_FBDIV_MIN, PLL_FBDIV_MAX);
	return *prate * fbdiv;
}

static unsigned long vcu_pll_recalc_rate(struct clk_hw *hw,
					 unsigned long parent_rate)
{
	struct vcu_pll *pll = to_vcu_pll(hw);
	u32 fbdiv, data, reg;
	unsigned long rate, frac;

	reg = readl(pll->pll_ctrl);
	fbdiv = (reg >> VCU_PLL_CTRL_FBDIV_SHIFT) & VCU_PLL_CTRL_FBDIV_MASK;

	rate = parent_rate * fbdiv;
	if (xvcu_pll_frac_get_mode(hw) == PLL_MODE_FRAC) {
		data = (readl(pll->pll_ctrl + FRAC_OFFSET) & 0xFFFF);
		frac = (parent_rate * data) / FRAC_DIV;
		rate = rate + frac;
	}

	return rate;
}

static int vcu_pll_set_rate(struct clk_hw *hw, unsigned long rate,
			    unsigned long parent_rate)
{
	struct vcu_pll *pll = to_vcu_pll(hw);
	u32 fbdiv, reg;
	long rate_div, frac, m, f;

	if (xvcu_pll_frac_get_mode(hw) == PLL_MODE_FRAC) {
		rate_div = ((rate * FRAC_DIV) / parent_rate);
		m = rate_div / FRAC_DIV;
		f = rate_div % FRAC_DIV;
		m = clamp_t(u32, m, (PLL_FBDIV_MIN), (PLL_FBDIV_MAX));
		rate = parent_rate * m;
		frac = (parent_rate * f) / FRAC_DIV;
		reg = readl(pll->pll_ctrl);
		reg &= ~(VCU_PLL_CTRL_FBDIV_MASK << VCU_PLL_CTRL_FBDIV_SHIFT);
		reg |= m << VCU_PLL_CTRL_FBDIV_SHIFT;
		writel(reg, pll->pll_ctrl);

		reg = readl(pll->pll_ctrl + FRAC_OFFSET);
		reg &= ~0xFFFF;
		reg |= (f & 0xFFFF);
		writel(reg, pll->pll_ctrl + FRAC_OFFSET);

		return (rate + frac);
	}

	fbdiv = DIV_ROUND_CLOSEST(rate, parent_rate);
	fbdiv = clamp_t(u32, fbdiv, PLL_FBDIV_MIN, PLL_FBDIV_MAX);
	reg = readl(pll->pll_ctrl);
	reg &= ~(VCU_PLL_CTRL_FBDIV_MASK << VCU_PLL_CTRL_FBDIV_SHIFT);
	reg |= fbdiv << VCU_PLL_CTRL_FBDIV_SHIFT;
	writel(reg, pll->pll_ctrl);

	return parent_rate * fbdiv;
}

static const struct clk_ops vcu_pll_ops = {
	.enable = xvcu_pll_enable,
	.disable = xvcu_pll_disable,
	.is_enabled = xvcu_pll_is_enabled,
	.round_rate = vcu_pll_round_rate,
	.recalc_rate = vcu_pll_recalc_rate,
	.set_rate = vcu_pll_set_rate,
};

/**
 * xvcu_register_pll - Register VCU PLL
 * @dev:	VCU clock device
 * @name:	PLL name
 * @parent:	PLL parent
 * @reg_base:	PLL register base address
 * @flags:	Hardware specific flags
 *
 * Register PLL to CCF.
 *
 * Return: Clock hardware for generated clock
 */
static struct clk_hw *xvcu_register_pll(struct device *dev, const char *name,
					const char *parent,
					void __iomem *reg_base,
					unsigned long flags)
{
	struct vcu_pll *pll;
	struct clk_hw *hw;
	struct clk_init_data init;
	int ret;

	init.name = name;
	init.parent_names = &parent;
	init.ops = &vcu_pll_ops;
	init.num_parents = 1;
	init.flags = flags;

	pll = devm_kmalloc(dev, sizeof(*pll), GFP_KERNEL);
	if (!pll)
		return ERR_PTR(-ENOMEM);

	pll->hw.init = &init;
	pll->pll_ctrl = reg_base + VCU_PLL_CTRL;
	pll->pll_status = reg_base + VCU_PLL_STATUS;
	pll->pll_cfg = reg_base + VCU_PLL_CFG;
	pll->lockbit = VCU_PLL_STATUS_LOCK_STATUS_MASK;

	hw = &pll->hw;
	ret = devm_clk_hw_register(dev, hw);
	if (ret)
		return ERR_PTR(ret);

	clk_hw_set_rate_range(hw, FVCO_MIN, FVCO_MAX);
	if (ret < 0)
		pr_err("%s:ERROR clk_set_rate_range failed %d\n", name, ret);

	return hw;
}

/**
 * register_vcu_leaf_clocks - Register VCU leaf clocks
 * @dev:		VCU clock device
 * @name:		Clock name
 * @parents:		Clock parents
 * @nparents:		Clock parent count
 * @default_parent:	Default parent to set
 * @reg:		Clock control register address
 * @lock:		Clock register access lock
 *
 * Register VCU leaf clocks. These clocks are MCU/core
 * encoder and decoder clocks. Topology for these clocks
 * are Mux, Divisor and Gate.
 *
 * Return: Clock hardware for the generated gate clock
 */
static struct clk_hw *register_vcu_leaf_clocks(struct device *dev,
					       const char *name,
					       const char * const *parents,
					       u8 nparents,
					       struct clk *default_parent,
					       void __iomem *reg,
					       spinlock_t *lock)
{
	char *clk_mux, *clk_div;
	struct clk_hw *hw;

	clk_mux = devm_kasprintf(dev, GFP_KERNEL, "%s%s", name, "_mux");
	hw = clk_hw_register_mux(dev, clk_mux, parents, nparents,
				 CLK_SET_RATE_PARENT |
				 CLK_SET_RATE_NO_REPARENT,
				 reg, VCU_SRCSEL_SHIFT, 1, 0, lock);

	if (default_parent)
		clk_set_parent(hw->clk, default_parent);

	clk_div = devm_kasprintf(dev, GFP_KERNEL, "%s%s", name, "_div");
	xvcu_register_divider(dev, clk_div, clk_mux,
			      CLK_SET_RATE_PARENT |
			      CLK_SET_RATE_NO_REPARENT,
			      reg, VCU_PLL_DIVISOR_SHIFT, 6,
			      CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO |
			      CLK_DIVIDER_ROUND_CLOSEST,
			      lock);

	return clk_hw_register_gate(dev, name, clk_div,
				    CLK_SET_RATE_PARENT,
				    reg, 12, 0, lock);
}

/**
 * unregister_vcu_leaf_clocks - Unegister VCU leaf clocks
 * @hw:		VCU leaf clock hardware
 *
 * Unregister VCU leaf clocks. These clocks are MCU/core
 * encoder and decoder clocks. Unregister clocks in order
 * from gate, div and mux maintaining their parent dependency.
 *
 */
static void unregister_vcu_leaf_clocks(struct clk_hw *hw)
{
	struct clk_hw *parent;

	parent = clk_hw_get_parent(hw);
	clk_hw_unregister_gate(hw);
	hw = parent;

	parent = clk_hw_get_parent(hw);
	clk_hw_unregister_divider(hw);
	hw = parent;

	clk_hw_unregister_mux(hw);
}

/**
 * xvcu_clock_init - Initialize VCU clocks
 * @dev:	VCU clock device
 * @reg_base:	Clock register base address
 *
 * Register VCU PLL and clocks and add VCU to clock provider list.
 *
 * Return: 0 on success else error code.
 */
static int xvcu_clock_init(struct device *dev, void __iomem *reg_base)
{
	struct clk_hw *hw;
	struct clk *ref_clk;
	const char *parent;
	u32 vcu_pll_ctrl, clkoutdiv;
	int i;

	ref_clk = devm_clk_get(dev, "pll_ref");
	if (IS_ERR(ref_clk)) {
		dev_err(dev, "failed to get pll_ref clock\n");
		return PTR_ERR(ref_clk);
	}

	vcu_clk_data = devm_kzalloc(dev, sizeof(*vcu_clk_data) +
				    sizeof(*vcu_clk_data->hws) * clk_max,
				    GFP_KERNEL);
	if (!vcu_clk_data)
		return -ENOMEM;

	parent = __clk_get_name(ref_clk);
	hw = xvcu_register_pll(dev, "vcu_pll", parent, reg_base,
			       CLK_SET_RATE_NO_REPARENT);
	if (IS_ERR(hw)) {
		dev_err(dev, "VCU PLL registration failed\n");
		return PTR_ERR(hw);
	}

	/*
	 * The divide-by-2 should be always enabled (== 1) to meet the timing
	 * in the design. Otherwise, it's an error
	 */
	vcu_pll_ctrl = readl(reg_base + VCU_PLL_CTRL);
	clkoutdiv = vcu_pll_ctrl >> VCU_PLL_CTRL_CLKOUTDIV_SHIFT;
	clkoutdiv = clkoutdiv & VCU_PLL_CTRL_CLKOUTDIV_MASK;
	if (clkoutdiv != 1) {
		dev_err(dev, "clkoutdiv is invalid\n");
		return -EINVAL;
	}

	vcu_clk_data->hws[vcu_pll_half] =
		clk_hw_register_fixed_factor(dev, "vcu_pll_half", "vcu_pll",
					     CLK_SET_RATE_NO_REPARENT |
					     CLK_SET_RATE_PARENT,
					     1, 2);

	vcu_clk_data->hws[vcu_core_enc] =
		register_vcu_leaf_clocks(dev, "vcu_core_enc_clk",
					 vcu_mux_parents, 2,
					 vcu_clk_data->hws[vcu_pll_half]->clk,
					 reg_base + VCU_ENC_CORE_CTRL,
					 &core_enc_lock);
	vcu_clk_data->hws[vcu_core_dec] =
		register_vcu_leaf_clocks(dev, "vcu_core_dec_clk",
					 vcu_mux_parents, 2,
					 vcu_clk_data->hws[vcu_pll_half]->clk,
					 reg_base + VCU_DEC_CORE_CTRL,
					 &core_dec_lock);
	vcu_clk_data->hws[mcu_core_enc] =
		register_vcu_leaf_clocks(dev, "mcu_core_enc_clk",
					 vcu_mux_parents, 2,
					 vcu_clk_data->hws[vcu_pll_half]->clk,
					 reg_base + VCU_ENC_MCU_CTRL,
					 &mcu_enc_lock);
	vcu_clk_data->hws[mcu_core_dec] =
		register_vcu_leaf_clocks(dev, "mcu_core_dec_clk",
					 vcu_mux_parents, 2,
					 vcu_clk_data->hws[vcu_pll_half]->clk,
					 reg_base + VCU_DEC_MCU_CTRL,
					 &mcu_dec_lock);

	for (i = 0; i < clk_max; i++) {
		if (IS_ERR(vcu_clk_data->hws[i])) {
			dev_err(dev, "clk %d: register failed with %ld\n",
				i, PTR_ERR(vcu_clk_data->hws[i]));
		}
	}

	vcu_clk_data->num = clk_max;
	return of_clk_add_hw_provider(dev->of_node, of_clk_hw_onecell_get,
				      vcu_clk_data);
}

static int xvcu_clk_probe(struct platform_device *pdev)
{
	struct xvcu_device *xvcu = dev_get_drvdata(pdev->dev.parent);
	int ret;

	ret = xvcu_clock_init(pdev->dev.parent, xvcu->vcu_slcr_ba);
	if (ret)
		dev_err(&pdev->dev, "clock init fail with error %d\n", ret);
	else
		dev_dbg(&pdev->dev, "clock init successful\n");

	return ret;
}

static int xvcu_clk_remove(struct platform_device *pdev)
{
	unregister_vcu_leaf_clocks(vcu_clk_data->hws[vcu_core_enc]);
	unregister_vcu_leaf_clocks(vcu_clk_data->hws[vcu_core_dec]);
	unregister_vcu_leaf_clocks(vcu_clk_data->hws[mcu_core_enc]);
	unregister_vcu_leaf_clocks(vcu_clk_data->hws[mcu_core_dec]);
	clk_hw_unregister(vcu_clk_data->hws[vcu_pll_half]);
	of_clk_del_provider(pdev->dev.parent->of_node);

	devm_kfree(pdev->dev.parent, vcu_clk_data);

	return 0;
}

static struct platform_driver xvcu_clk_driver = {
	.driver = {
		.name = "xilinx-vcu-clk",
	},
	.probe = xvcu_clk_probe,
	.remove = xvcu_clk_remove,
};

module_platform_driver(xvcu_clk_driver);

MODULE_AUTHOR("Rajan Vaja <rajan.vaja@xilinx.com>");
MODULE_DESCRIPTION("Xilinx VCU clock Driver");
MODULE_LICENSE("GPL v2");
MODULE_ALIAS("platform:xilinx-vcu-clk");