drivers/clock_control/clock_control_litex.c

/*
 * Copyright (c) 2020 Antmicro <www.antmicro.com>
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/types.h>
#include <device.h>
#include <devicetree.h>
#include <drivers/clock_control.h>
#include <drivers/clock_control/clock_control_litex.h>
#include "clock_control_litex.h"
#include <logging/log.h>
#include <logging/log_ctrl.h>
#include <sys/util.h>
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <kernel.h>

LOG_MODULE_REGISTER(CLK_CTRL_LITEX, CONFIG_CLOCK_CONTROL_LOG_LEVEL);

static struct litex_clk_device *ldev;	/* global struct for whole driver */
static struct litex_clk_clkout *clkouts;/* clkout array for whole driver */

/* All DRP regs addresses and sizes */
static struct litex_drp_reg drp[] = {
	{DRP_ADDR_RESET,  DRP_SIZE_RESET},
	{DRP_ADDR_LOCKED, DRP_SIZE_LOCKED},
	{DRP_ADDR_READ,   DRP_SIZE_READ},
	{DRP_ADDR_WRITE,  DRP_SIZE_WRITE},
	{DRP_ADDR_DRDY,   DRP_SIZE_DRDY},
	{DRP_ADDR_ADR,    DRP_SIZE_ADR},
	{DRP_ADDR_DAT_W,  DRP_SIZE_DAT_W},
	{DRP_ADDR_DAT_R,  DRP_SIZE_DAT_R},
};

struct litex_clk_regs_addr litex_clk_regs_addr_init(void)
{
	struct litex_clk_regs_addr m;
	uint32_t i, addr;

	addr = CLKOUT0_REG1;
	for (i = 0; i <= CLKOUT_MAX; i++) {
		if (i == 5) {
		/*
		 *special case because CLKOUT5 have its reg addresses
		 *placed lower than other CLKOUTs
		 */
			m.clkout[5].reg1 = CLKOUT5_REG1;
			m.clkout[5].reg2 = CLKOUT5_REG2;
		} else {
			m.clkout[i].reg1 = addr;
			addr++;
			m.clkout[i].reg2 = addr;
			addr++;
		}
	}
	return m;
}

/*
 * These lookup tables are taken from:
 * https://github.com/Digilent/Zybo-hdmi-out/blob/b991fff6e964420ae3c00c3dbee52f2ad748b3ba/sdk/displaydemo/src/dynclk/dynclk.h
 *
 *	2015 Copyright Digilent Incorporated
 *	Author: Sam Bobrowicz
 *
 */

/* MMCM loop filter lookup table */
static const uint32_t litex_clk_filter_table[] = {
	 0b0001011111,
	 0b0001010111,
	 0b0001111011,
	 0b0001011011,
	 0b0001101011,
	 0b0001110011,
	 0b0001110011,
	 0b0001110011,
	 0b0001110011,
	 0b0001001011,
	 0b0001001011,
	 0b0001001011,
	 0b0010110011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001010011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0001100011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010010011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011,
	 0b0010100011
};

/* MMCM lock detection lookup table */
static const uint64_t litex_clk_lock_table[] = {
	0b0011000110111110100011111010010000000001,
	0b0011000110111110100011111010010000000001,
	0b0100001000111110100011111010010000000001,
	0b0101101011111110100011111010010000000001,
	0b0111001110111110100011111010010000000001,
	0b1000110001111110100011111010010000000001,
	0b1001110011111110100011111010010000000001,
	0b1011010110111110100011111010010000000001,
	0b1100111001111110100011111010010000000001,
	0b1110011100111110100011111010010000000001,
	0b1111111111111000010011111010010000000001,
	0b1111111111110011100111111010010000000001,
	0b1111111111101110111011111010010000000001,
	0b1111111111101011110011111010010000000001,
	0b1111111111101000101011111010010000000001,
	0b1111111111100111000111111010010000000001,
	0b1111111111100011111111111010010000000001,
	0b1111111111100010011011111010010000000001,
	0b1111111111100000110111111010010000000001,
	0b1111111111011111010011111010010000000001,
	0b1111111111011101101111111010010000000001,
	0b1111111111011100001011111010010000000001,
	0b1111111111011010100111111010010000000001,
	0b1111111111011001000011111010010000000001,
	0b1111111111011001000011111010010000000001,
	0b1111111111010111011111111010010000000001,
	0b1111111111010101111011111010010000000001,
	0b1111111111010101111011111010010000000001,
	0b1111111111010100010111111010010000000001,
	0b1111111111010100010111111010010000000001,
	0b1111111111010010110011111010010000000001,
	0b1111111111010010110011111010010000000001,
	0b1111111111010010110011111010010000000001,
	0b1111111111010001001111111010010000000001,
	0b1111111111010001001111111010010000000001,
	0b1111111111010001001111111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001,
	0b1111111111001111101011111010010000000001
};
/* End of copied code */

/* Helper function for filter lookup table */
static inline uint32_t litex_clk_lookup_filter(uint32_t glob_mul)
{
	return litex_clk_filter_table[glob_mul - 1];
}

/* Helper function for lock lookup table */
static inline uint64_t litex_clk_lookup_lock(uint32_t glob_mul)
{
	return litex_clk_lock_table[glob_mul - 1];
}

static inline void litex_clk_set_reg(uint32_t reg, uint32_t val)
{
	litex_write((uint32_t *)drp[reg].addr, drp[reg].size, val);
}

static inline uint32_t litex_clk_get_reg(uint32_t reg)
{
	return litex_read((uint32_t *)drp[reg].addr, drp[reg].size);
}

static inline void litex_clk_assert_reg(uint32_t reg)
{
	int assert = (1 << (drp[reg].size * BITS_PER_BYTE)) - 1;

	litex_clk_set_reg(reg, assert);
}

static inline void litex_clk_deassert_reg(uint32_t reg)
{
	litex_clk_set_reg(reg, ZERO_REG);
}

static int litex_clk_wait(uint32_t reg)
{
	uint32_t timeout;

	__ASSERT(reg == DRP_LOCKED || reg == DRP_DRDY, "Unsupported register! Please provide DRP_LOCKED or DRP_DRDY");

	if (reg == DRP_LOCKED) {
		timeout = ldev->timeout.lock;
	} else {
		timeout = ldev->timeout.drdy;
	}
	/*Waiting for signal to assert in reg*/
	while (!litex_clk_get_reg(reg) && timeout) {
		timeout--;
		k_sleep(K_MSEC(1));
	}
	if (timeout == 0) {
		LOG_WRN("Timeout occured when waiting for the register: 0x%x", reg);
		return -ETIME;
	}
	return 0;
}

/* Read value written in given internal MMCM register*/
static int litex_clk_get_DO(uint8_t clk_reg_addr, uint16_t *res)
{
	int ret;

	litex_clk_set_reg(DRP_ADR, clk_reg_addr);
	litex_clk_assert_reg(DRP_READ);

	litex_clk_deassert_reg(DRP_READ);
	ret = litex_clk_wait(DRP_DRDY);
	if (ret != 0) {
		return ret;
	}

	*res = litex_clk_get_reg(DRP_DAT_R);

	return 0;
}

/* Get global divider and multiplier values and update global config */
static int litex_clk_update_global_config(void)
{
	int ret;
	uint16_t divreg, mult2;
	uint8_t low_time, high_time;

	ret = litex_clk_get_DO(CLKFBOUT_REG2, &mult2);
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_get_DO(DIV_REG, &divreg);
	if (ret != 0) {
		return ret;
	}

	if (mult2 & (NO_CNT_MASK << NO_CNT_POS)) {
		ldev->g_config.mul = 1;
	} else {
		uint16_t mult1;

		ret = litex_clk_get_DO(CLKFBOUT_REG1, &mult1);
		if (ret != 0) {
			return ret;
		}
		low_time = mult1 & HL_TIME_MASK;
		high_time = (mult1 >> HIGH_TIME_POS) & HL_TIME_MASK;
		ldev->g_config.mul = low_time + high_time;
	}

	if (divreg & (NO_CNT_MASK << NO_CNT_DIVREG_POS)) {
		ldev->g_config.div = 1;
	} else {
		low_time = divreg & HL_TIME_MASK;
		high_time = (divreg >> HIGH_TIME_POS) & HL_TIME_MASK;
		ldev->g_config.div = low_time + high_time;
	}

	return 0;
}

static uint64_t litex_clk_calc_global_frequency(uint32_t mul, uint32_t div)
{
	uint64_t f;

	f = (uint64_t)ldev->sys_clk_freq * (uint64_t)mul;
	f /= div;

	return f;
}

/* Calculate frequency with real global params and update global config */
static uint64_t litex_clk_get_real_global_frequency(void)
{
	uint64_t f;

	litex_clk_update_global_config();
	f = litex_clk_calc_global_frequency(ldev->g_config.mul,
					    ldev->g_config.div);
	ldev->g_config.freq = f;
	ldev->ts_g_config.div = ldev->g_config.div;
	ldev->ts_g_config.mul = ldev->g_config.mul;
	ldev->ts_g_config.freq = ldev->g_config.freq;

	return f;
}

/* Return dividers of given CLKOUT */
static int litex_clk_get_clkout_divider(struct litex_clk_clkout *lcko,
					uint32_t *divider, uint32_t *fract_cnt)
{
	struct litex_clk_regs_addr drp_addr = litex_clk_regs_addr_init();
	int ret;
	uint16_t div, frac;
	uint8_t clkout_nr = lcko->id;
	uint8_t low_time, high_time;

	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg1, &div);
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg2, &frac);
	if (ret != 0) {
		return ret;
	}

	low_time = div & HL_TIME_MASK;
	high_time = (div >> HIGH_TIME_POS) & HL_TIME_MASK;
	*divider = low_time + high_time;
	*fract_cnt = (frac >> FRAC_POS) & FRAC_MASK;

	return 0;
}

/* Debug functions */
#ifdef CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG

static void litex_clk_check_DO(char *reg_name, uint8_t clk_reg_addr,
					       uint16_t *res)
{
	int ret;

	ret = litex_clk_get_DO(clk_reg_addr, res);
	if (ret != 0)
		LOG_ERR("%s: read error: %d", reg_name, ret);
	else
		LOG_DBG("%s:  0x%x", reg_name, *res);
}

static void litex_clk_print_general_regs(void)
{
	uint16_t power_reg, div_reg, clkfbout_reg1, clkfbout_reg2,
	lock_reg1, lock_reg2, lock_reg3, filt_reg1, filt_reg2;

	litex_clk_check_DO("POWER_REG", POWER_REG, &power_reg);
	litex_clk_check_DO("DIV_REG", DIV_REG, &div_reg);
	litex_clk_check_DO("MUL_REG1", CLKFBOUT_REG1, &clkfbout_reg1);
	litex_clk_check_DO("MUL_REG2", CLKFBOUT_REG2, &clkfbout_reg2);
	litex_clk_check_DO("LOCK_REG1", LOCK_REG1, &lock_reg1);
	litex_clk_check_DO("LOCK_REG2", LOCK_REG2, &lock_reg2);
	litex_clk_check_DO("LOCK_REG3", LOCK_REG3, &lock_reg3);
	litex_clk_check_DO("FILT_REG1", FILT_REG1, &filt_reg1);
	litex_clk_check_DO("FILT_REG2", FILT_REG2, &filt_reg2);
}

static void litex_clk_print_clkout_regs(uint8_t clkout, uint8_t reg1,
							uint8_t reg2)
{
	uint16_t clkout_reg1, clkout_reg2;
	char reg_name[16];

	sprintf(reg_name, "CLKOUT%u REG1", clkout);
	litex_clk_check_DO(reg_name, reg1, &clkout_reg1);

	sprintf(reg_name, "CLKOUT%u REG2", clkout);
	litex_clk_check_DO(reg_name, reg2, &clkout_reg2);
}

static void litex_clk_print_all_regs(void)
{
	struct litex_clk_regs_addr drp_addr = litex_clk_regs_addr_init();
	uint32_t i;

	litex_clk_print_general_regs();
	for (i = 0; i < ldev->nclkout; i++) {
		litex_clk_print_clkout_regs(i, drp_addr.clkout[i].reg1,
					       drp_addr.clkout[i].reg2);
	}
}

static void litex_clk_print_params(struct litex_clk_clkout *lcko)
{
	LOG_DBG("CLKOUT%d DUMP:", lcko->id);
	LOG_DBG("Defaults:");
	LOG_DBG("f: %u d: %u/%u p: %u",
		lcko->def.freq, lcko->def.duty.num,
		lcko->def.duty.den, lcko->def.phase);
	LOG_DBG("Config to set:");
	LOG_DBG("div: %u freq: %u duty: %u/%u phase: %d per_off: %u",
		lcko->ts_config.div, lcko->ts_config.freq,
		lcko->ts_config.duty.num, lcko->ts_config.duty.den,
		lcko->ts_config.phase, lcko->config.period_off);
	LOG_DBG("Config:");
	LOG_DBG("div: %u freq: %u duty: %u/%u phase: %d per_off: %u",
		lcko->config.div, lcko->config.freq,
		lcko->config.duty.num, lcko->config.duty.den,
		lcko->config.phase, lcko->config.period_off);
	LOG_DBG("Divide group:");
	LOG_DBG("e: %u ht: %u lt: %u nc: %u",
		lcko->div.edge, lcko->div.high_time,
		lcko->div.low_time, lcko->div.no_cnt);
	LOG_DBG("Frac group:");
	LOG_DBG("f: %u fen: %u fwff: %u fwfr: %u pmf: %u",
		lcko->frac.frac, lcko->frac.frac_en, lcko->frac.frac_wf_f,
		lcko->frac.frac_wf_r, lcko->frac.phase_mux_f);
	LOG_DBG("Phase group:");
	LOG_DBG("dt: %u pm: %u mx: %u",
		lcko->phase.delay_time, lcko->phase.phase_mux, lcko->phase.mx);
}

static void litex_clk_print_all_params(void)
{
	uint32_t c;

	LOG_DBG("Global Config to set:");
	LOG_DBG("freq: %llu mul: %u div: %u",
		ldev->ts_g_config.freq, ldev->ts_g_config.mul,
					ldev->ts_g_config.div);
	LOG_DBG("Global Config:");
	LOG_DBG("freq: %llu mul: %u div: %u",
		ldev->g_config.freq, ldev->g_config.mul, ldev->g_config.div);
	for (c = 0; c < ldev->nclkout; c++) {
		litex_clk_print_params(&ldev->clkouts[c]);
	}
}
#endif /* CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG */

/* Returns raw value ready to be written into MMCM */
static inline uint16_t litex_clk_calc_DI(uint16_t DO_val, uint16_t mask,
							  uint16_t bitset)
{
	uint16_t DI_val;

	DI_val = DO_val & mask;
	DI_val |= bitset;

	return DI_val;
}

/* Sets calculated DI value into DI DRP register */
static int litex_clk_set_DI(uint16_t DI_val)
{
	int ret;

	litex_clk_set_reg(DRP_DAT_W, DI_val);
	litex_clk_assert_reg(DRP_WRITE);
	litex_clk_deassert_reg(DRP_WRITE);
	ret = litex_clk_wait(DRP_DRDY);
	return ret;
}

/*
 * Change register value as specified in arguments
 *
 * mask:		preserve or zero MMCM register bits
 *			by selecting 1 or 0 on desired specific mask positions
 * bitset:		set those bits in MMCM register which are 1 in bitset
 * clk_reg_addr:	internal MMCM address of control register
 *
 */
static int litex_clk_change_value(uint16_t mask, uint16_t bitset,
						 uint8_t clk_reg_addr)
{
	uint16_t DO_val, DI_val;
	int ret;

	litex_clk_assert_reg(DRP_RESET);

	ret = litex_clk_get_DO(clk_reg_addr, &DO_val);
	if (ret != 0) {
		return ret;
	}
	DI_val = litex_clk_calc_DI(DO_val, mask, bitset);
	ret = litex_clk_set_DI(DI_val);
	if (ret != 0) {
		return ret;
	}
#ifdef CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG
	DI_val = litex_clk_get_reg(DRP_DAT_W);
	LOG_DBG("set 0x%x under: 0x%x", DI_val, clk_reg_addr);
#endif
	litex_clk_deassert_reg(DRP_DAT_W);
	litex_clk_deassert_reg(DRP_RESET);
	ret = litex_clk_wait(DRP_LOCKED);
	return ret;
}

/*
 * Set register values for given CLKOUT
 *
 * clkout_nr:		clock output number
 * mask_regX:		preserve or zero MMCM register X bits
 *			by selecting 1 or 0 on desired specific mask positions
 * bitset_regX:		set those bits in MMCM register X which are 1 in bitset
 *
 */
static int litex_clk_set_clock(uint8_t clkout_nr, uint16_t mask_reg1,
			      uint16_t bitset_reg1, uint16_t mask_reg2,
						 uint16_t bitset_reg2)
{
	struct litex_clk_regs_addr drp_addr = litex_clk_regs_addr_init();
	int ret;

	if (!(mask_reg2 == FULL_REG_16 && bitset_reg2 == ZERO_REG)) {
		ret = litex_clk_change_value(mask_reg2, bitset_reg2,
					     drp_addr.clkout[clkout_nr].reg2);
		if (ret != 0) {
			return ret;
		}
	}
	if (!(mask_reg1 == FULL_REG_16 && bitset_reg1 == ZERO_REG)) {
		ret = litex_clk_change_value(mask_reg1, bitset_reg1,
					     drp_addr.clkout[clkout_nr].reg1);
		if (ret != 0) {
			return ret;
		}
	}

	return 0;
}

/* Set global divider for all CLKOUTs */
static int litex_clk_set_divreg(void)
{
	int ret;
	uint8_t no_cnt = 0, edge = 0, ht = 0, lt = 0,
		div = ldev->ts_g_config.div;
	uint16_t bitset = 0;

	if (div == 1) {
		no_cnt = 1;
	} else {
		ht = div / 2;
		lt = ht;
		edge = div % 2;
		if (edge) {
			lt += edge;
		}
	}

	bitset = (edge << EDGE_DIVREG_POS)	|
		 (no_cnt << NO_CNT_DIVREG_POS)	|
		 (ht << HIGH_TIME_POS)		|
		 (lt << LOW_TIME_POS);

	ret = litex_clk_change_value(KEEP_IN_DIV, bitset, DIV_REG);
	if (ret != 0) {
		return ret;
	}

	ldev->g_config.div = div;
	LOG_DBG("Global divider set to %u", div);

	return 0;
}

/* Set global multiplier for all CLKOUTs */
static int litex_clk_set_mulreg(void)
{
	int ret;
	uint8_t no_cnt = 0, edge = 0, ht = 0, lt = 0,
		mul = ldev->ts_g_config.mul;
	uint16_t bitset1 = 0;

	if (mul == 1) {
		no_cnt = 1;
	} else {
		ht = mul / 2;
		lt = ht;
		edge = mul % 2;
		if (edge) {
			lt += edge;
		}
	}

	bitset1 = (ht << HIGH_TIME_POS) |
		  (lt << LOW_TIME_POS);

	ret = litex_clk_change_value(KEEP_IN_MUL_REG1, bitset1, CLKFBOUT_REG1);
	if (ret != 0) {
		return ret;
	}

	if (edge || no_cnt) {
		uint16_t bitset2 = (edge << EDGE_POS)	|
				(no_cnt << NO_CNT_POS);

		ret = litex_clk_change_value(KEEP_IN_MUL_REG2,
				       bitset2, CLKFBOUT_REG2);
		if (ret != 0) {
			return ret;
		}
	}

	ldev->g_config.mul = mul;
	LOG_DBG("Global multiplier set to %u", mul);

	return 0;
}

static int litex_clk_set_filt(void)
{
	uint16_t filt_reg;
	uint32_t filt, mul;
	int ret;

	mul = ldev->g_config.mul;
	filt = litex_clk_lookup_filter(mul);

	/*
	 * Preparing and setting filter register values
	 * according to reg map form Xilinx XAPP888
	 */
	filt_reg = (((filt >> 9) & 0x1) << 15) |
		   (((filt >> 7) & 0x3) << 11) |
		   (((filt >> 6) & 0x1) << 8);
	ret = litex_clk_change_value(FILT1_MASK, filt_reg, FILT_REG1);
	if (ret != 0)  {
		return ret;
	}

	filt_reg = (((filt >> 5) & 0x1) << 15) |
		   (((filt >> 3) & 0x3) << 11) |
		   (((filt >> 1) & 0x3) << 7) |
		   (((filt) & 0x1) << 4);
	ret = litex_clk_change_value(FILT2_MASK, filt_reg, FILT_REG2);

	return ret;
}

static int litex_clk_set_lock(void)
{
	uint16_t lock_reg;
	uint32_t mul;
	uint64_t lock;
	int ret;

	mul = ldev->g_config.mul;
	lock = litex_clk_lookup_lock(mul);

	/*
	 * Preparing and setting lock register values
	 * according to reg map form Xilinx XAPP888
	 */
	lock_reg = (lock >> 20) & 0x3FF;
	ret = litex_clk_change_value(LOCK1_MASK, lock_reg, LOCK_REG1);
	if (ret != 0) {
		return ret;
	}

	lock_reg = (((lock >> 30) & 0x1F) << 10) |
		     (lock & 0x3FF);
	ret = litex_clk_change_value(LOCK23_MASK, lock_reg, LOCK_REG2);
	if (ret != 0) {
		return ret;
	}

	lock_reg = (((lock >> 35) & 0x1F) << 10) |
		   ((lock >> 10) & 0x3FF);
	ret = litex_clk_change_value(LOCK23_MASK, lock_reg, LOCK_REG3);

	return ret;
}

/* Set all multiplier-related regs: mul, filt and lock regs */
static int litex_clk_set_mul(void)
{
	int ret;

	ret = litex_clk_set_mulreg();
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_set_filt();
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_set_lock();
	return ret;
}

static int litex_clk_set_both_globs(void)
{
	/*
	 * we need to check what change first to prevent
	 * getting our VCO_FREQ out of possible range
	 */
	uint64_t vco_freq;
	int ret;

	/* div-first case */
	vco_freq = litex_clk_calc_global_frequency(
					ldev->g_config.mul,
					ldev->ts_g_config.div);
	if (vco_freq > ldev->vco.max || vco_freq < ldev->vco.min) {
		/* div-first not safe */
		vco_freq = litex_clk_calc_global_frequency(
					ldev->ts_g_config.mul,
					ldev->g_config.div);
		if (vco_freq > ldev->vco.max || vco_freq < ldev->vco.min) {
			/* mul-first not safe */
			ret = litex_clk_set_divreg();
			/* Ignore timeout because we expect that to happen */
			if (ret != -ETIME && ret != 0) {
				return ret;
			} else if (ret == -ETIME) {
				ldev->g_config.div = ldev->ts_g_config.div;
				LOG_DBG("Global divider set to %u",
					 ldev->g_config.div);
			}
			ret = litex_clk_set_mul();
			if (ret != 0) {
				return ret;
			}
		} else {
			/* mul-first safe */
			ret = litex_clk_set_mul();
			if (ret != 0) {
				return ret;
			}
			ret = litex_clk_set_divreg();
			if (ret != 0) {
				return ret;
			}
		}
	} else {
		/* div-first safe */
		ret = litex_clk_set_divreg();
		if (ret != 0) {
			return ret;
		}
		ret = litex_clk_set_mul();
		if (ret != 0) {
			return ret;
		}
	}
	return 0;
}

/* Set global divider, multiplier, filt and lock values */
static int litex_clk_set_globs(void)
{
	int ret;
	uint8_t set_div = 0,
	   set_mul = 0;

	set_div = ldev->ts_g_config.div != ldev->g_config.div;
	set_mul = ldev->ts_g_config.mul != ldev->g_config.mul;

	if (set_div || set_mul) {
		if (set_div && set_mul) {
			ret = litex_clk_set_both_globs();
			if (ret != 0) {
				return ret;
			}
		} else if (set_div) {
			/* set divider only */
			ret = litex_clk_set_divreg();
			if (ret != 0) {
				return ret;
			}
		} else {
			/* set multiplier only */
			ret = litex_clk_set_mul();
			if (ret != 0) {
				return ret;
			}
		}
		ldev->g_config.freq = ldev->ts_g_config.freq;
	}
	return 0;
}

/* Round scaled value*/
static inline uint32_t litex_round(uint32_t val, uint32_t mod)
{
	if (val % mod > mod / 2) {
		return val / mod + 1;
	}
	return val / mod;
}

/*
 *	Duty Cycle
 */

/* Returns accurate duty ratio of given clkout*/
int litex_clk_get_duty_cycle(struct litex_clk_clkout *lcko,
			     struct clk_duty *duty)
{
	struct litex_clk_regs_addr drp_addr = litex_clk_regs_addr_init();
	int ret;
	uint32_t divider;
	uint16_t clkout_reg1, clkout_reg2;
	uint8_t clkout_nr, high_time, edge, no_cnt, frac_en, frac_cnt;

	clkout_nr = lcko->id;
	/* Check if divider is off */
	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg2, &clkout_reg2);
	if (ret != 0) {
		return ret;
	}

	edge = (clkout_reg2 >> EDGE_POS) & EDGE_MASK;
	no_cnt = (clkout_reg2 >> NO_CNT_POS) & NO_CNT_MASK;
	frac_en = (clkout_reg2 >> FRAC_EN_POS) & FRAC_EN_MASK;
	frac_cnt = (clkout_reg2 >> FRAC_POS) & FRAC_MASK;

	/* get duty 50% when divider is off or fractional is enabled */
	if (no_cnt || (frac_en && frac_cnt)) {
		duty->num = 1;
		duty->den = 2;
		return 0;
	}

	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg1, &clkout_reg1);
	if (ret != 0) {
		return ret;
	}

	divider = clkout_reg1 & HL_TIME_MASK;
	high_time = (clkout_reg1 >> HIGH_TIME_POS) & HL_TIME_MASK;
	divider += high_time;

	/* Scaling to consider edge control bit */
	duty->num = high_time * 10 + edge * 5;
	duty->den = (divider + edge) * 10;

	return 0;
}

/* Calculates duty cycle for given ratio in percent, 1% accuracy */
static inline uint8_t litex_clk_calc_duty_percent(struct clk_duty *duty)
{
	uint32_t div, duty_ratio, ht;

	ht = duty->num;
	div = duty->den;
	duty_ratio = ht * 10000 / div;

	return (uint8_t)litex_round(duty_ratio, 100);
}

/* Calculate necessary values for setting duty cycle in normal mode */
static int litex_clk_calc_duty_normal(struct litex_clk_clkout *lcko,
					 int calc_new)
{
	struct clk_duty duty;
	int delta_d;
	uint32_t ht_aprox, synth_duty, min_d;
	uint8_t high_time_it, edge_it, high_duty,
	   divider = lcko->config.div;

	if (calc_new) {
		duty = lcko->ts_config.duty;
	} else {
		litex_clk_get_duty_cycle(lcko, &duty);
	}

	high_duty = litex_clk_calc_duty_percent(&duty);
	min_d = INT_MAX;
	/* check if duty is available to set */
	ht_aprox = high_duty * divider;

	if (ht_aprox > ((HIGH_LOW_TIME_REG_MAX * 100) + 50) ||
		       ((HIGH_LOW_TIME_REG_MAX * 100) + 50) <
			(divider * 100) - ht_aprox) {
		return -EINVAL;
	}

	/* to prevent high_time == 0 or low_time == 0 */
	for (high_time_it = 1; high_time_it < divider; high_time_it++) {
		for (edge_it = 0; edge_it < 2; edge_it++) {
			synth_duty = (high_time_it * 100 + 50 * edge_it) /
								  divider;
			delta_d = synth_duty - high_duty;
			delta_d = abs(delta_d);
			/* check if low_time won't be above acceptable range */
			if (delta_d < min_d && (divider - high_time_it) <=
						  HIGH_LOW_TIME_REG_MAX) {
				min_d = delta_d;
				lcko->div.high_time = high_time_it;
				lcko->div.low_time = divider - high_time_it;
				lcko->div.edge = edge_it;
				lcko->config.duty.num = high_time_it * 100 + 50
								     * edge_it;
				lcko->config.duty.den = divider * 100;
			}
		}
	}
	/*
	 * Calculating values in normal mode,
	 * clear control bits of fractional part
	 */
	lcko->frac.frac_wf_f = 0;
	lcko->frac.frac_wf_r = 0;

	return 0;
}

/* Calculates duty high_time for given divider and ratio */
static inline int litex_clk_calc_duty_high_time(struct clk_duty *duty,
						   uint32_t divider)
{
	uint32_t high_duty;

	high_duty = litex_clk_calc_duty_percent(duty) * divider;

	return litex_round(high_duty, 100);
}

/* Set duty cycle with given ratio */
static int litex_clk_set_duty_cycle(struct litex_clk_clkout *lcko,
			     struct clk_duty *duty)
{
	int ret;
	uint16_t bitset1, bitset2;
	uint8_t clkout_nr = lcko->id,
	   *edge = &lcko->div.edge,
	   *high_time = &lcko->div.high_time,
	    high_duty = litex_clk_calc_duty_percent(duty),
	   *low_time = &lcko->div.low_time;

	if (lcko->frac.frac == 0) {
		int ret;

		lcko->ts_config.duty = *duty;
		LOG_DBG("CLKOUT%d: setting duty: %u/%u",
			lcko->id, duty->num, duty->den);
		ret = litex_clk_calc_duty_normal(lcko, true);
		if (ret != 0) {
			LOG_ERR("CLKOUT%d: cannot set %d%% duty cycle",
				clkout_nr, high_duty);
			return ret;
		}
	} else {
		LOG_ERR("CLKOUT%d: cannot set duty cycle when fractional divider enabled",
								     clkout_nr);
		return -EACCES;
	}

	bitset1 = (*high_time << HIGH_TIME_POS) |
		  (*low_time << LOW_TIME_POS);
	bitset2 = (*edge << EDGE_POS);

	LOG_DBG("SET DUTY CYCLE: e:%u ht:%u lt:%u\nbitset1: 0x%x bitset2: 0x%x",
		*edge, *high_time, *low_time, bitset1, bitset2);

	ret = litex_clk_set_clock(clkout_nr, REG1_DUTY_MASK, bitset1,
					     REG2_DUTY_MASK, bitset2);
	if (ret != 0) {
		return ret;
	}

	LOG_INF("CLKOUT%d: set duty: %d%%", lcko->id,
		litex_clk_calc_duty_percent(&lcko->config.duty));
	return 0;
}

/*
 *	Phase
 */

/* Calculate necessary values for setting phase in normal mode */
static int litex_clk_calc_phase_normal(struct litex_clk_clkout *lcko)
{
	uint64_t period_buff;
	uint32_t post_glob_div_f, global_period,  clkout_period,
	    *period_off = &lcko->ts_config.period_off;
	uint8_t divider = lcko->config.div;
	/* ps unit */

	post_glob_div_f = (uint32_t)litex_clk_get_real_global_frequency();
	period_buff = PICOS_IN_SEC;
	period_buff /= post_glob_div_f;
	global_period = (uint32_t)period_buff;
	clkout_period = global_period * divider;

	if (lcko->ts_config.phase != 0) {
		int synth_phase, delta_p, min_p, p_o;
		uint8_t delay, p_m;

		*period_off = litex_round(clkout_period * (*period_off), 10000);

		if (*period_off / global_period > DELAY_TIME_MAX) {
			return -EINVAL;
		}

		min_p = INT_MAX;
		p_o = *period_off;
		/* Delay_time: (0-63) */
		for (delay = 0; delay <= DELAY_TIME_MAX; delay++) {
			/* phase_mux: (0-7) */
			for (p_m = 0; p_m <= PHASE_MUX_MAX; p_m++) {
				synth_phase = (delay * global_period) +
				((p_m * ((global_period * 100) / 8) / 100));

				delta_p = synth_phase - p_o;
				delta_p = abs(delta_p);
				if (delta_p < min_p) {
					min_p = delta_p;
					lcko->phase.phase_mux = p_m;
					lcko->phase.delay_time = delay;
					lcko->config.period_off = synth_phase;
				}
			}
		}
	} else {
		/* Don't change phase offset*/
		lcko->phase.phase_mux = 0;
		lcko->phase.delay_time = 0;
	}
	/*
	 * Calculating values in normal mode,
	 * fractional control bits need to be zero
	 */
	lcko->frac.phase_mux_f = 0;

	return 0;
}

/* Convert phase offset to positive lower than 360 deg. and calculate period */
static int litex_clk_prepare_phase(struct litex_clk_clkout *lcko)
{
	int *phase = &lcko->ts_config.phase;

	*phase %= 360;

	if (*phase < 0) {
		*phase += 360;
	}

	lcko->ts_config.period_off = ((*phase * 10000) / 360);

	return 0;
}

/* Calculate necessary values for setting phase */
static int litex_clk_calc_phase(struct litex_clk_clkout *lcko)
{
	litex_clk_prepare_phase(lcko);

	return litex_clk_calc_phase_normal(lcko);
}

/* Returns phase-specific values of given clock output */
static int litex_clk_get_phase_data(struct litex_clk_clkout *lcko,
				    uint8_t *phase_mux, uint8_t *delay_time)
{
	struct litex_clk_regs_addr drp_addr = litex_clk_regs_addr_init();
	int ret;
	uint16_t r1, r2;
	uint8_t clkout_nr = lcko->id;

	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg1, &r1);
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_get_DO(drp_addr.clkout[clkout_nr].reg2, &r2);
	if (ret != 0) {
		return ret;
	}

	*phase_mux = (r1 >> PHASE_MUX_POS) & PHASE_MUX_MASK;
	*delay_time = (r2 >> DELAY_TIME_POS) & HL_TIME_MASK;

	return 0;
}

/* Returns phase of given clock output in time offset */
int litex_clk_get_phase(struct litex_clk_clkout *lcko)
{
	uint64_t period_buff;
	uint32_t divider = 0, fract_cnt, post_glob_div_f,
	    pm, global_period, clkout_period, period;
	uint8_t phase_mux = 0, delay_time = 0;

	litex_clk_get_phase_data(lcko, &phase_mux, &delay_time);
	litex_clk_get_clkout_divider(lcko, &divider, &fract_cnt);

	post_glob_div_f = (uint32_t)litex_clk_get_real_global_frequency();
	period_buff = PICOS_IN_SEC;
	period_buff /= post_glob_div_f;
	/* ps unit */
	global_period = (uint32_t)period_buff;
	clkout_period = global_period * divider;

	pm = (phase_mux * global_period * 1000) / PHASE_MUX_RES_FACTOR;
	pm = litex_round(pm, 1000);

	period = delay_time * global_period + pm;

	period = period * 1000 / clkout_period;
	period = period * 360;

	return litex_round(period, 1000);
}

/* Returns phase of given clock output in degrees */
int litex_clk_get_phase_deg(struct litex_clk_clkout *lcko)
{
	uint64_t post_glob_div_f, buff, clkout_period;

	post_glob_div_f = (uint32_t)litex_clk_get_real_global_frequency();
	buff = PICOS_IN_SEC;
	buff /= post_glob_div_f;
	clkout_period = (uint32_t)buff;
	clkout_period *= lcko->config.div;

	buff = lcko->config.period_off * 1000 / clkout_period;
	buff *= 360;
	buff = litex_round(buff, 1000);

	return (int)buff;
}
/* Sets phase given in degrees on given clock output */
int litex_clk_set_phase(struct litex_clk_clkout *lcko, int degrees)
{
	int ret;
	uint16_t bitset1, bitset2, reg2_mask;
	uint8_t *phase_mux = &lcko->phase.phase_mux,
	   *delay_time = &lcko->phase.delay_time,
	   clkout_nr = lcko->id;

	lcko->ts_config.phase = degrees;
	reg2_mask = REG2_PHASE_MASK;
	LOG_DBG("CLKOUT%d: setting phase: %u deg", lcko->id, degrees);

	ret = litex_clk_calc_phase(lcko);
	if (ret != 0) {
		LOG_ERR("CLKOUT%d: phase offset %d deg is too high",
			 clkout_nr, degrees);
		return ret;
	}

	bitset1 = (*phase_mux << PHASE_MUX_POS);
	bitset2 = (*delay_time << DELAY_TIME_POS);

	ret = litex_clk_set_clock(clkout_nr, REG1_PHASE_MASK, bitset1,
						   reg2_mask, bitset2);
	if (ret != 0) {
		return ret;
	}
	lcko->config.phase = litex_clk_get_phase_deg(lcko);
	LOG_INF("CLKOUT%d: set phase: %d deg", lcko->id, lcko->config.phase);
	LOG_DBG("SET PHASE: pm:%u dt:%u\nbitset1: 0x%x bitset2: 0x%x",
		*phase_mux, *delay_time, bitset1, bitset2);

	return 0;
}

/*
 *	Frequency
 */

/* Returns rate in Hz */
static inline uint32_t litex_clk_calc_rate(struct litex_clk_clkout *lcko)
{
	uint64_t f = litex_clk_calc_global_frequency(ldev->ts_g_config.mul,
						  ldev->ts_g_config.div);

	f /= lcko->config.div;

	return (uint32_t)f;
}

/*
 * Written since there is no pow() in math.h. Only for exponent
 * and base above 0. Used for calculating scaling factor for
 * frequency margin
 *
 */
static uint32_t litex_clk_pow(uint32_t base, uint32_t exp)
{
	int ret = 1;

	while (exp--) {
		ret *= base;
	}

	return ret;
}

/* Returns true when possible to set frequency with given global settings */
static int litex_clk_calc_clkout_params(struct litex_clk_clkout *lcko,
					 uint64_t vco_freq)
{
	int delta_f;
	uint64_t m, clk_freq = 0;
	uint32_t d, margin = 1;

	if (lcko->margin.exp) {
		margin = litex_clk_pow(10, lcko->margin.exp);
	}

	lcko->div.no_cnt = 0;

	for (d = lcko->clkout_div.min; d <= lcko->clkout_div.max; d++) {
		clk_freq = vco_freq;
		clk_freq /= d;
		m = lcko->ts_config.freq * lcko->margin.m;
		/* Scale margin according to its exponent */
		if (lcko->margin.exp) {
			m /= margin;
		}

		delta_f = clk_freq - lcko->ts_config.freq;
		delta_f = abs(delta_f);
		if (delta_f <= m) {
			lcko->config.freq = (uint32_t)clk_freq;
			if (lcko->config.div != d) {
				ldev->update_clkout[lcko->id] = 1;
			}
			lcko->config.div = d;
			/* for sake of completeness */
			lcko->ts_config.div = d;
			/* we are not using fractional divider */
			lcko->frac.frac_en = 0;
			lcko->frac.frac = 0;
			if (d == 1) {
				lcko->div.no_cnt = 1;
			}
			LOG_DBG("CLKOUT%d: freq:%u div:%u gdiv:%u gmul:%u",
				 lcko->id, lcko->config.freq, lcko->config.div,
				 ldev->ts_g_config.div, ldev->ts_g_config.mul);
			return true;
		}
	}

	return false;
}

/* Compute dividers for all active clock outputs */
static int litex_clk_calc_all_clkout_params(uint64_t vco_freq)
{
	struct litex_clk_clkout *lcko;
	uint32_t c;

	for (c = 0; c < ldev->nclkout; c++) {
		lcko = &ldev->clkouts[c];
		if (!litex_clk_calc_clkout_params(lcko, vco_freq)) {
			return false;
		}
	}
	return true;
}

/* Calculate parameters for whole active part of MMCM */
static int litex_clk_calc_all_params(void)
{
	uint32_t div, mul;
	uint64_t vco_freq = 0;

	for (div = ldev->divclk.min; div <= ldev->divclk.max; div++) {
		ldev->ts_g_config.div = div;
		for (mul = ldev->clkfbout.max; mul >= ldev->clkfbout.min;
								 mul--) {
			int bellow, above, all_valid = true;

			vco_freq = (uint64_t)ldev->sys_clk_freq * (uint64_t)mul;
			vco_freq /= div;
			bellow = vco_freq < (ldev->vco.min
					     * (1 + ldev->vco_margin));
			above = vco_freq > (ldev->vco.max
					    * (1 - ldev->vco_margin));

			if (!bellow && !above) {
				all_valid = litex_clk_calc_all_clkout_params
								     (vco_freq);
				if (all_valid) {
					ldev->ts_g_config.mul = mul;
					ldev->ts_g_config.freq = vco_freq;
					LOG_DBG("GLOBAL: freq:%llu g_div:%u g_mul:%u",
						ldev->ts_g_config.freq,
						ldev->ts_g_config.div,
						ldev->ts_g_config.mul);
					return 0;
				}
			}
		}
	}
	LOG_ERR("Cannot find correct settings for all clock outputs!");
	return -ENOTSUP;
}

int litex_clk_check_rate_range(struct litex_clk_clkout *lcko, uint32_t rate)
{
	uint64_t max, min, m;
	uint32_t div, margin;

	m = rate * lcko->margin.m;
	if (lcko->margin.exp) {
		margin = litex_clk_pow(10, lcko->margin.exp);
	}

	max = (uint64_t)ldev->sys_clk_freq * (uint64_t)ldev->clkfbout.max;
	div = ldev->divclk.min * lcko->clkout_div.min;
	max /= div;
	max += m;

	min = ldev->sys_clk_freq * ldev->clkfbout.min;
	div = ldev->divclk.max * lcko->clkout_div.max;
	min /= div;

	if (min < m) {
		min = 0;
	} else {
		min -= m;
	}

	if ((uint64_t)rate < min || (uint64_t)rate > max) {
		return -EINVAL;
	}
	return 0;
}

/* Returns closest available clock rate in Hz */
long litex_clk_round_rate(struct litex_clk_clkout *lcko, unsigned long rate)
{
	int ret;

	ret = litex_clk_check_rate_range(lcko, rate);
	if (ret != 0) {
		return -EINVAL;
	}

	lcko->ts_config.freq = rate;

	ret = litex_clk_calc_all_params();
	if (ret != 0) {
		return ret;
	}

	return litex_clk_calc_rate(lcko);
}

int litex_clk_write_rate(struct litex_clk_clkout *lcko)
{
	int ret;
	uint16_t bitset1, bitset2;
	uint8_t *divider = &lcko->config.div,
	   *edge = &lcko->div.edge,
	   *high_time = &lcko->div.high_time,
	   *low_time = &lcko->div.low_time,
	   *no_cnt = &lcko->div.no_cnt,
	   *frac = &lcko->frac.frac,
	   *frac_en = &lcko->frac.frac_en,
	   *frac_wf_r = &lcko->frac.frac_wf_r;

	bitset1 = (*high_time << HIGH_TIME_POS)	|
		  (*low_time << LOW_TIME_POS);

	bitset2 = (*frac << FRAC_POS)		|
		  (*frac_en << FRAC_EN_POS)	|
		  (*frac_wf_r << FRAC_WF_R_POS)	|
		  (*edge << EDGE_POS)		|
		  (*no_cnt << NO_CNT_POS);

	LOG_DBG("SET RATE: div:%u f:%u fwfr:%u fen:%u nc:%u e:%u ht:%u lt:%u\nbitset1: 0x%x bitset2: 0x%x",
		*divider, *frac, *frac_wf_r, *frac_en,
		*no_cnt, *edge, *high_time, *low_time, bitset1, bitset2);

	ret = litex_clk_set_clock(lcko->id, REG1_FREQ_MASK, bitset1,
					    REG2_FREQ_MASK, bitset2);
	if (ret != 0) {
		return ret;
	}

	ldev->update_clkout[lcko->id] = 0;

	return 0;
}

int litex_clk_update_clkouts(void)
{
	struct litex_clk_clkout *lcko;
	int ret;
	uint8_t c;

	for (c = 0; c < ldev->nclkout; c++) {
		if (ldev->update_clkout[c]) {
			lcko = &ldev->clkouts[c];
			ret = litex_clk_calc_duty_normal(lcko, false);
			if (ret != 0) {
				return ret;
			}
			ret = litex_clk_write_rate(lcko);
			if (ret != 0) {
				return ret;
			}
			LOG_INF("CLKOUT%d: updated rate: %u to %u HZ",
					lcko->id, lcko->ts_config.freq,
						     lcko->config.freq);
		}
	}

	return 0;
}
/* Set closest available clock rate in Hz, parent_rate ignored */
int litex_clk_set_rate(struct litex_clk_clkout *lcko, unsigned long rate)
{
	int ret;

	LOG_DBG("CLKOUT%d: setting rate: %lu", lcko->id, rate);
	ret = litex_clk_round_rate(lcko, rate);
	if (ret < 0) {
		return ret;
	}
	ret = litex_clk_set_globs();
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_calc_duty_normal(lcko, false);
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_write_rate(lcko);
	if (ret != 0) {
		return ret;
	}
	LOG_INF("CLKOUT%d: set rate: %u HZ", lcko->id, lcko->config.freq);
	ret = litex_clk_update_clkouts();
	if (ret != 0) {
		return ret;
	}

#ifdef CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG
	litex_clk_print_all_params();
	litex_clk_print_all_regs();
#endif /* CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG */

	return 0;
}

/* Set default clock value from device tree for given clkout*/
static int litex_clk_set_def_clkout(int clkout_nr)
{
	struct litex_clk_clkout *lcko = &ldev->clkouts[clkout_nr];
	int ret;

	ret = litex_clk_set_rate(lcko, lcko->def.freq);
	if (ret != 0) {
		return ret;
	}
	ret = litex_clk_set_duty_cycle(lcko, &lcko->def.duty);
	if (ret != 0) {
		return ret;
	}
	return litex_clk_set_phase(lcko, lcko->def.phase);
}

static int litex_clk_set_all_def_clkouts(void)
{
	int c, ret;

	for (c = 0; c < ldev->nclkout; c++) {
		ret = litex_clk_set_def_clkout(c);
		if (ret != 0) {
			return ret;
		}
	}
	return 0;
}

/*
 * Returns parameters of given clock output
 *
 * clock:		device structure for driver
 * sub_system:		pointer to struct litex_clk_clkout
 *			casted to clock_control_subsys with
 *			all clkout parameters
 */
static int litex_clk_get_subsys_rate(const struct device *clock,
				     clock_control_subsys_t sys, uint32_t *rate)
{
	struct litex_clk_setup *setup = sys;
	struct litex_clk_clkout *lcko;

	lcko = &ldev->clkouts[setup->clkout_nr];
	*rate = litex_clk_calc_rate(lcko);

	return 0;
}

static enum clock_control_status litex_clk_get_status(const struct device *dev,
						     clock_control_subsys_t sys)
{
	struct litex_clk_setup *setup = sys;
	struct clk_duty duty;
	struct litex_clk_clkout *lcko;
	int ret;

	lcko = &ldev->clkouts[setup->clkout_nr];

	setup->rate = litex_clk_calc_rate(lcko);
	ret = litex_clk_get_duty_cycle(lcko, &duty);
	if (ret != 0) {
		return ret;
	}
	setup->duty = litex_clk_calc_duty_percent(&duty);
	setup->phase = litex_clk_get_phase(lcko);

	return CLOCK_CONTROL_STATUS_ON;
}

static inline int litex_clk_on(const struct device *dev, clock_control_subsys_t sys)
{
	struct litex_clk_setup *setup = sys;
	struct clk_duty duty;
	struct litex_clk_clkout *lcko;
	uint8_t duty_perc;
	int ret;

	lcko = &ldev->clkouts[setup->clkout_nr];

	if (lcko->config.freq != setup->rate) {
		ret = litex_clk_set_rate(lcko, setup->rate);
		if (ret != 0) {
			return ret;
		}
	}
	if (lcko->config.phase != setup->phase) {
		ret = litex_clk_set_phase(lcko, setup->phase);
		if (ret != 0) {
			return ret;
		}
	}
	duty_perc = litex_clk_calc_duty_percent(&lcko->config.duty);
	if (duty_perc != setup->duty) {
		duty.num = setup->duty;
		duty.den = 100;
		ret = litex_clk_set_duty_cycle(lcko, &duty);
		if (ret != 0) {
			return ret;
		}
	}
	return 0;
}

static inline int litex_clk_off(const struct device *dev,
				clock_control_subsys_t sub_system)
{
	return litex_clk_change_value(ZERO_REG, ZERO_REG, POWER_REG);
}

static const struct clock_control_driver_api litex_clk_api = {
	.on = litex_clk_on,
	.off = litex_clk_off,
	.get_rate = litex_clk_get_subsys_rate,
	.get_status = litex_clk_get_status
};

static void litex_clk_dts_clkout_ranges_read(struct litex_clk_range *clkout_div)
{
	clkout_div->min = CLKOUT_DIVIDE_MIN;
	clkout_div->max = CLKOUT_DIVIDE_MAX;
}

static int litex_clk_dts_timeout_read(struct litex_clk_timeout *timeout)
{
	/* Read wait_lock timeout from device property*/
	timeout->lock = LOCK_TIMEOUT;
	if (timeout->lock < 1) {
		LOG_ERR("LiteX CLK driver cannot wait shorter than ca. 1ms\n");
		return -EINVAL;
	}

	/* Read wait_drdy timeout from device property*/
	timeout->drdy = DRDY_TIMEOUT;
	if (timeout->drdy < 1) {
		LOG_ERR("LiteX CLK driver cannot wait shorter than ca. 1ms\n");
		return -EINVAL;
	}

	return 0;
}

static int litex_clk_dts_clkouts_read(void)
{
	struct litex_clk_range clkout_div;
	struct litex_clk_clkout *lcko;

	litex_clk_dts_clkout_ranges_read(&clkout_div);

#if CLKOUT_EXIST(0) == 1
		CLKOUT_INIT(0)
#endif
#if CLKOUT_EXIST(1) == 1
		CLKOUT_INIT(1)
#endif
#if CLKOUT_EXIST(2) == 1
		CLKOUT_INIT(2)
#endif
#if CLKOUT_EXIST(3) == 1
		CLKOUT_INIT(3)
#endif
#if CLKOUT_EXIST(4) == 1
		CLKOUT_INIT(4)
#endif
#if CLKOUT_EXIST(5) == 1
		CLKOUT_INIT(5)
#endif
#if CLKOUT_EXIST(6) == 1
		CLKOUT_INIT(6)
#endif
	return 0;
}

static void litex_clk_init_clkouts(void)
{
	struct litex_clk_clkout *lcko;
	int i;

	for (i = 0; i < ldev->nclkout; i++) {
		lcko = &ldev->clkouts[i];
		lcko->base = ldev->base;
		/* mark defaults to set */
		lcko->ts_config.freq = lcko->def.freq;
		lcko->ts_config.duty = lcko->def.duty;
		lcko->ts_config.phase = lcko->def.phase;
	}
}

static int litex_clk_dts_cnt_clocks(void)
{
	return NCLKOUT;
}

static void litex_clk_dts_global_ranges_read(void)
{
	ldev->divclk.min = DIVCLK_DIVIDE_MIN;
	ldev->divclk.max = DIVCLK_DIVIDE_MAX;
	ldev->clkfbout.min = CLKFBOUT_MULT_MIN;
	ldev->clkfbout.max = CLKFBOUT_MULT_MAX;
	ldev->vco.min = VCO_FREQ_MIN;
	ldev->vco.max = VCO_FREQ_MAX;
	ldev->vco_margin = VCO_MARGIN;
}

static int litex_clk_dts_global_read(void)
{
	int ret;

	ldev->sys_clk_freq = SYS_CLOCK_FREQUENCY;

	ldev->nclkout = litex_clk_dts_cnt_clocks();

	clkouts = k_malloc(sizeof(struct litex_clk_clkout) * ldev->nclkout);
	ldev->update_clkout = k_malloc(sizeof(uint8_t) * ldev->nclkout);
	if (!clkouts || !ldev->update_clkout) {
		LOG_ERR("CLKOUT memory allocation failure!");
		return -ENOMEM;
	}
	ldev->clkouts = clkouts;

	ret = litex_clk_dts_timeout_read(&ldev->timeout);
	if (ret != 0) {
		return ret;
	}

	litex_clk_dts_global_ranges_read();

	return 0;
}

static int litex_clk_init_glob_clk(void)
{
	int ret;

	/* Power on MMCM module */
	ret = litex_clk_change_value(FULL_REG_16, FULL_REG_16, POWER_REG);
	if (ret != 0) {
		LOG_ERR("MMCM initialization failure, ret: %d", ret);
		return ret;
	}

	return 0;
}
/* Enable module, set global divider, multiplier, default clkout parameters */
static int litex_clk_init(const struct device *dev)
{
	int ret;

	ldev = k_malloc(sizeof(struct litex_clk_device));
	if (ldev == NULL) {
		return -ENOMEM;
	}

	ldev->base = (uint32_t *)DRP_BASE;
	if (ldev->base == NULL) {
		return -EIO;
	}

	ret = litex_clk_dts_global_read();
	if (ret != 0) {
		return ret;
	}

	ret = litex_clk_dts_clkouts_read();
	if (ret != 0) {
		return ret;
	}

	litex_clk_init_clkouts();

	ret = litex_clk_init_glob_clk();
	if (ret != 0) {
		return ret;
	}

	ret = litex_clk_set_all_def_clkouts();
	if (ret != 0) {
		return ret;
	}

#ifdef CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG
	litex_clk_print_all_params();
	litex_clk_print_all_regs();
#endif /* CONFIG_CLOCK_CONTROL_LOG_LEVEL_DBG */

	LOG_INF("LiteX Clock Control driver initialized");
	return 0;
}

static const struct litex_clk_device ldev_init = {
	.base = (uint32_t *)DRP_BASE,
	.timeout = {LOCK_TIMEOUT, DRDY_TIMEOUT},
	.divclk = {DIVCLK_DIVIDE_MIN, DIVCLK_DIVIDE_MAX},
	.clkfbout = {CLKFBOUT_MULT_MIN, CLKFBOUT_MULT_MAX},
	.vco = {VCO_FREQ_MIN, VCO_FREQ_MAX},
	.sys_clk_freq = SYS_CLOCK_FREQUENCY,
	.vco_margin = VCO_MARGIN,
	.nclkout = NCLKOUT
};

DEVICE_DT_DEFINE(DT_NODELABEL(clock0), &litex_clk_init, NULL,
		    NULL, &ldev_init, POST_KERNEL,
		    CONFIG_KERNEL_INIT_PRIORITY_DEVICE, &litex_clk_api);