diff -drupN a/drivers/clk/sunxi/clk-ac100.c b/drivers/clk/sunxi/clk-ac100.c --- a/drivers/clk/sunxi/clk-ac100.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/clk/sunxi/clk-ac100.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2013 Allwinnertech, kevin.z.m + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ +#include +#include +#include +#include "clk-sunxi.h" +#include "clk-factors.h" +#include "clk-periph.h" +#ifdef CONFIG_ARCH_SUN9IW1 +#include "clk-sun9iw1.h" +#endif +#ifdef CONFIG_ARCH_SUN8IW6 +#include "clk-sun8iw6.h" +#endif +#include + +#define CK32K_OUT_CTRL1 0xC1 +#define CK32K_OUT_CTRL2 0xC2 +#define CK32K_OUT_CTRL3 0xC3 + +/* +* SUNXI_CLK_PERIPH(name, mux_reg, mux_shift, mux_width, div_reg, div_mshift, div_mwidth, div_nshift, div_nwidth, gate_flags, enable_reg, reset_reg, bus_gate_reg, drm_gate_reg, enable_shift, reset_shift, bus_gate_shift, dram_gate_shift,lock,com_gate,com_gate_off) +*/ +SUNXI_CLK_PERIPH(ac10032k1, CK32K_OUT_CTRL1, 4, 1, CK32K_OUT_CTRL1, 5, 3, 1, 3, 0, CK32K_OUT_CTRL1, 0, 0, 0, 0, 0, 0, 0, NULL, NULL, 0); +SUNXI_CLK_PERIPH(ac10032k2, CK32K_OUT_CTRL2, 4, 1, CK32K_OUT_CTRL2, 5, 3, 1, 3, 0, CK32K_OUT_CTRL2, 0, 0, 0, 0, 0, 0, 0, NULL, NULL, 0); +SUNXI_CLK_PERIPH(ac10032k3, CK32K_OUT_CTRL3, 4, 1, CK32K_OUT_CTRL3, 5, 3, 1, 3, 0, CK32K_OUT_CTRL3, 0, 0, 0, 0, 0, 0, 0, NULL, NULL, 0); +static const char *ac10032k_parents[] = {"32k_rtc", "4m_adda"}; + +#ifndef CONFIG_ARCH_SUN8IW6 +static struct periph_init_data sunxi_ac100_init[] = { + {"ac10032k1", CLK_GET_RATE_NOCACHE, ac10032k_parents, ARRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k1}, + {"ac10032k2", CLK_GET_RATE_NOCACHE, ac10032k_parents, RRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k2}, + {"ac10032k3", CLK_GET_RATE_NOCACHE, ac10032k_parents, ARRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k3}, +}; +#else +/*add flag CLK_IGNORE_SYNCBOOT for SUN8IW6 platform*/ +static struct periph_init_data sunxi_ac100_init[] = { + {"ac10032k1", CLK_GET_RATE_NOCACHE|CLK_IGNORE_SYNCBOOT, ac10032k_parents, ARRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k1}, + {"ac10032k2", CLK_GET_RATE_NOCACHE|CLK_IGNORE_SYNCBOOT, ac10032k_parents, ARRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k2}, + {"ac10032k3", CLK_GET_RATE_NOCACHE|CLK_IGNORE_SYNCBOOT, ac10032k_parents, ARRAY_SIZE(ac10032k_parents), &sunxi_clk_periph_ac10032k3}, +}; +#endif + +static unsigned int ac100_m_factor[] = {1, 2, 4, 8, 16, 32, 64, 122}; +static unsigned int ac100_n_factor[] = {1, 2, 4, 8, 16, 32, 64, 122}; + +static unsigned long sunxi_ac100_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) +{ + unsigned long reg = 0; + struct sunxi_clk_periph *periph = to_clk_periph(hw); + struct sunxi_clk_periph_div *divider = &periph->divider; + unsigned long div, div_m = 0, div_n = 0; + u64 rate = parent_rate; + + if (!divider->reg) + return parent_rate; + + reg = periph_readl(periph, divider->reg); + if (divider->mwidth) + div_m = GET_BITS(divider->mshift, divider->mwidth, reg); + + if (divider->nwidth) + div_n = GET_BITS(divider->nshift, divider->nwidth, reg); + + if (reg & 0x100) + div = ac100_m_factor[div_m] * ac100_n_factor[div_n]; + else + div = ac100_n_factor[div_n]; + + do_div(rate, div); + + return rate; +} +static long sunxi_ac100_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *prate) +{ + int i = 0, j = 0, m_max = 0; + int m = 0, n = 0; + unsigned long cur_rate = 0, new_rate = 0; + unsigned long cur_delta = 0, new_delta = 0; + u32 parent_rate = *prate; + + if (*prate == 4000000) + m_max = 1; + else + m_max = 8; + + for (i = 0; i < m_max; i++) { + for (j = 0; j < 8; j++) { + new_rate = parent_rate/(ac100_m_factor[i] * ac100_n_factor[j]); + new_delta = (new_rate > rate) ? (new_rate-rate) : (rate-new_rate); + cur_delta = (cur_rate > rate) ? (cur_rate-rate) : (rate-cur_rate); + if (new_delta < cur_delta) { + cur_rate = new_rate; + m = i; + n = j; + } + } + } + + return cur_rate; +} + +static int __sunxi_clk_periph_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) +{ + struct sunxi_clk_periph *periph = to_clk_periph(hw); + struct sunxi_clk_periph_div *divider = &periph->divider; + int i = 0, j = 0, m_max = 0; + int m = 0, n = 0; + unsigned long cur_rate = 0, new_rate = 0; + unsigned long cur_delta = 0, new_delta = 0; + u32 reg = 0; + + if (parent_rate == 4000000) + m_max = 1; + else + m_max = 8; + + for (i = 0; i < m_max; i++) { + for (j = 0; j < 8; j++) { + new_rate = parent_rate/(ac100_m_factor[i]*ac100_n_factor[j]); + new_delta = (new_rate > rate) ? (new_rate-rate) : (rate-new_rate); + cur_delta = (cur_rate > rate) ? (cur_rate-rate) : (rate-cur_rate); + if (new_delta < cur_delta) { + cur_rate = new_rate; + m = i; + n = j; + } + } + } + + reg = periph_readl(periph, divider->reg); + if (divider->mwidth) + reg = SET_BITS(divider->mshift, divider->mwidth, reg, m); + + if (divider->nwidth) + reg = SET_BITS(divider->nshift, divider->nwidth, reg, n); + + periph_writel(periph, reg, divider->reg); + + return 0; +} + +static u32 ac100_readl(void __iomem *reg) +{ + arisc_rsb_block_cfg_t rsb_data; + unsigned char addr; + unsigned int val; + + addr = (unsigned char)((unsigned long)reg); + rsb_data.len = 1; + rsb_data.datatype = RSB_DATA_TYPE_HWORD; + rsb_data.msgattr = ARISC_MESSAGE_ATTR_SOFTSYN; + rsb_data.devaddr = RSB_RTSADDR_AC100; + rsb_data.regaddr = &addr; + rsb_data.data = &val; + + /* read registers */ + if (arisc_rsb_read_block_data(&rsb_data)) + pr_err("%s(%d) err: read reg-0x%x failed", __func__, __LINE__, (unsigned int __force)reg); + + return val; +} + +static void ac100_writel(u32 val, void __iomem *reg) +{ + arisc_rsb_block_cfg_t rsb_data; + u16 data = (u16)val; + + rsb_data.len = 1; + rsb_data.datatype = RSB_DATA_TYPE_HWORD; + rsb_data.msgattr = ARISC_MESSAGE_ATTR_SOFTSYN; + rsb_data.devaddr = RSB_RTSADDR_AC100; + rsb_data.regaddr = (unsigned char *)® + rsb_data.data = (unsigned int *)&data; + +#ifdef CONFIG_ARCH_SUN9IW1 + if (((unsigned int __force)reg == 0xc1) && (!(val & 0x01))) { + pr_err("Warning!!! %s skip write %x to reg %x\n", __func__, val, (unsigned int __force)reg); + return; + } +#endif + /* write registers */ + if (arisc_rsb_write_block_data(&rsb_data)) + pr_err("%s(%d) err: write reg-0x%x failed", __func__, __LINE__, (unsigned int __force)reg); + +} + +static struct clk_ops ac100_clkops; +static struct sunxi_reg_ops ac100_regops; + +static int __init sunxi_ac100_clocks_setup(void) +{ + int i; + struct clk *clk; + struct device_node *np; + const struct of_device_id *matches = &__clk_of_table; + struct periph_init_data *periph; + + if (arisc_rsb_set_rtsaddr(RSB_DEVICE_SADDR7, RSB_RTSADDR_AC100)) { + pr_err("%s err: config codec failed\n", __func__); + return -1; + } + + pr_info("----------sunxi_ac100_clocks_setup------------\n"); + sunxi_clk_get_periph_ops(&ac100_clkops); + ac100_clkops.prepare = ac100_clkops.enable; + ac100_clkops.unprepare = ac100_clkops.disable; + ac100_clkops.enable = NULL; + ac100_clkops.disable = NULL; + ac100_clkops.recalc_rate = sunxi_ac100_recalc_rate; + ac100_clkops.round_rate = sunxi_ac100_round_rate; + ac100_clkops.set_rate = __sunxi_clk_periph_set_rate; + ac100_regops.reg_writel = ac100_writel; + ac100_regops.reg_readl = ac100_readl; + + for_each_matching_node(np, matches) { + for (i = 0; i < ARRAY_SIZE(sunxi_ac100_init); i++) { + periph = &sunxi_ac100_init[i]; + if (strcmp(periph->name, np->name)) + continue; + + periph->periph->priv_clkops = &ac100_clkops; + periph->periph->priv_regops = &ac100_regops; + clk = sunxi_clk_register_periph(periph, 0); + + if (!IS_ERR(clk)) { + clk_register_clkdev(clk, np->name, NULL); + of_clk_add_provider(np, of_clk_src_simple_get, clk); + } + } + } + return 0; +} + +static __maybe_unused int __init sunxi_init_ac100_clocks(void) +{ + struct clk *clk; + int i = 0; + struct periph_init_data *periph; + + if (arisc_rsb_set_rtsaddr(RSB_DEVICE_SADDR7, RSB_RTSADDR_AC100)) { + pr_err("%s err: config codec failed\n", __func__); + return -1; + } + + /* reigster source for AC100 32K */ + clk = clk_register_fixed_rate(NULL, "32k_rtc", NULL, CLK_IS_ROOT, 32768); + clk_register_clkdev(clk, "32k_rtc", NULL); + clk = clk_register_fixed_rate(NULL, "4m_adda", NULL, CLK_IS_ROOT, 4000000); + clk_register_clkdev(clk, "4m_adda", NULL); + + sunxi_clk_get_periph_ops(&ac100_clkops); + ac100_clkops.prepare = ac100_clkops.enable; + ac100_clkops.unprepare = ac100_clkops.disable; + ac100_clkops.enable = NULL; + ac100_clkops.disable = NULL; + ac100_clkops.recalc_rate = sunxi_ac100_recalc_rate; + ac100_clkops.round_rate = sunxi_ac100_round_rate; + ac100_clkops.set_rate = __sunxi_clk_periph_set_rate; + ac100_regops.reg_writel = ac100_writel; + ac100_regops.reg_readl = ac100_readl; + + /* register AC100 clock */ + for (i = 0; i < ARRAY_SIZE(sunxi_ac100_init); i++) { + periph = &sunxi_ac100_init[i]; + periph->periph->priv_clkops = &ac100_clkops; + periph->periph->priv_regops = &ac100_regops; + clk = sunxi_clk_register_periph(periph, sunxi_clk_base); + clk_register_clkdev(clk, periph->name, NULL); + } + + return 0; +} +subsys_initcall_sync(sunxi_ac100_clocks_setup);