clk/at91/clk-smd.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
0004  */
0005
0006 #include <linux/clk-provider.h>
0007 #include <linux/clkdev.h>
0008 #include <linux/clk/at91_pmc.h>
0009 #include <linux/of.h>
0010 #include <linux/mfd/syscon.h>
0011 #include <linux/regmap.h>
0012
0013 #include "pmc.h"
0014
0015 #define SMD_DIV_SHIFT       8
0016 #define SMD_MAX_DIV     0xf
0017
0018 struct at91sam9x5_clk_smd {
0019     struct clk_hw hw;
0020     struct regmap *regmap;
0021 };
0022
0023 #define to_at91sam9x5_clk_smd(hw) \
0024     container_of(hw, struct at91sam9x5_clk_smd, hw)
0025
0026 static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
0027                             unsigned long parent_rate)
0028 {
0029     struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
0030     unsigned int smdr;
0031     u8 smddiv;
0032
0033     regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
0034     smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;
0035
0036     return parent_rate / (smddiv + 1);
0037 }
0038
0039 static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate,
0040                       unsigned long *parent_rate)
0041 {
0042     unsigned long div;
0043     unsigned long bestrate;
0044     unsigned long tmp;
0045
0046     if (rate >= *parent_rate)
0047         return *parent_rate;
0048
0049     div = *parent_rate / rate;
0050     if (div > SMD_MAX_DIV)
0051         return *parent_rate / (SMD_MAX_DIV + 1);
0052
0053     bestrate = *parent_rate / div;
0054     tmp = *parent_rate / (div + 1);
0055     if (bestrate - rate > rate - tmp)
0056         bestrate = tmp;
0057
0058     return bestrate;
0059 }
0060
0061 static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
0062 {
0063     struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
0064
0065     if (index > 1)
0066         return -EINVAL;
0067
0068     regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,
0069                index ? AT91_PMC_SMDS : 0);
0070
0071     return 0;
0072 }
0073
0074 static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
0075 {
0076     struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
0077     unsigned int smdr;
0078
0079     regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
0080
0081     return smdr & AT91_PMC_SMDS;
0082 }
0083
0084 static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
0085                        unsigned long parent_rate)
0086 {
0087     struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
0088     unsigned long div = parent_rate / rate;
0089
0090     if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))
0091         return -EINVAL;
0092
0093     regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,
0094                (div - 1) << SMD_DIV_SHIFT);
0095
0096     return 0;
0097 }
0098
0099 static const struct clk_ops at91sam9x5_smd_ops = {
0100     .recalc_rate = at91sam9x5_clk_smd_recalc_rate,
0101     .round_rate = at91sam9x5_clk_smd_round_rate,
0102     .get_parent = at91sam9x5_clk_smd_get_parent,
0103     .set_parent = at91sam9x5_clk_smd_set_parent,
0104     .set_rate = at91sam9x5_clk_smd_set_rate,
0105 };
0106
0107 struct clk_hw * __init
0108 at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
0109                 const char **parent_names, u8 num_parents)
0110 {
0111     struct at91sam9x5_clk_smd *smd;
0112     struct clk_hw *hw;
0113     struct clk_init_data init;
0114     int ret;
0115
0116     smd = kzalloc(sizeof(*smd), GFP_KERNEL);
0117     if (!smd)
0118         return ERR_PTR(-ENOMEM);
0119
0120     init.name = name;
0121     init.ops = &at91sam9x5_smd_ops;
0122     init.parent_names = parent_names;
0123     init.num_parents = num_parents;
0124     init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
0125
0126     smd->hw.init = &init;
0127     smd->regmap = regmap;
0128
0129     hw = &smd->hw;
0130     ret = clk_hw_register(NULL, &smd->hw);
0131     if (ret) {
0132         kfree(smd);
0133         hw = ERR_PTR(ret);
0134     }
0135
0136     return hw;
0137 }