clk/at91/clk-programmable.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 PROG_ID_MAX     7
0016
0017 #define PROG_STATUS_MASK(id)    (1 << ((id) + 8))
0018 #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask)
0019 #define PROG_MAX_RM9200_CSS 3
0020
0021 struct clk_programmable {
0022     struct clk_hw hw;
0023     struct regmap *regmap;
0024     u32 *mux_table;
0025     u8 id;
0026     const struct clk_programmable_layout *layout;
0027     struct at91_clk_pms pms;
0028 };
0029
0030 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
0031
0032 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
0033                           unsigned long parent_rate)
0034 {
0035     struct clk_programmable *prog = to_clk_programmable(hw);
0036     const struct clk_programmable_layout *layout = prog->layout;
0037     unsigned int pckr;
0038     unsigned long rate;
0039
0040     regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
0041
0042     if (layout->is_pres_direct)
0043         rate = parent_rate / (PROG_PRES(layout, pckr) + 1);
0044     else
0045         rate = parent_rate >> PROG_PRES(layout, pckr);
0046
0047     return rate;
0048 }
0049
0050 static int clk_programmable_determine_rate(struct clk_hw *hw,
0051                        struct clk_rate_request *req)
0052 {
0053     struct clk_programmable *prog = to_clk_programmable(hw);
0054     const struct clk_programmable_layout *layout = prog->layout;
0055     struct clk_hw *parent;
0056     long best_rate = -EINVAL;
0057     unsigned long parent_rate;
0058     unsigned long tmp_rate = 0;
0059     int shift;
0060     int i;
0061
0062     for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
0063         parent = clk_hw_get_parent_by_index(hw, i);
0064         if (!parent)
0065             continue;
0066
0067         parent_rate = clk_hw_get_rate(parent);
0068         if (layout->is_pres_direct) {
0069             for (shift = 0; shift <= layout->pres_mask; shift++) {
0070                 tmp_rate = parent_rate / (shift + 1);
0071                 if (tmp_rate <= req->rate)
0072                     break;
0073             }
0074         } else {
0075             for (shift = 0; shift < layout->pres_mask; shift++) {
0076                 tmp_rate = parent_rate >> shift;
0077                 if (tmp_rate <= req->rate)
0078                     break;
0079             }
0080         }
0081
0082         if (tmp_rate > req->rate)
0083             continue;
0084
0085         if (best_rate < 0 ||
0086             (req->rate - tmp_rate) < (req->rate - best_rate)) {
0087             best_rate = tmp_rate;
0088             req->best_parent_rate = parent_rate;
0089             req->best_parent_hw = parent;
0090         }
0091
0092         if (!best_rate)
0093             break;
0094     }
0095
0096     if (best_rate < 0)
0097         return best_rate;
0098
0099     req->rate = best_rate;
0100     return 0;
0101 }
0102
0103 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)
0104 {
0105     struct clk_programmable *prog = to_clk_programmable(hw);
0106     const struct clk_programmable_layout *layout = prog->layout;
0107     unsigned int mask = layout->css_mask;
0108     unsigned int pckr = index;
0109
0110     if (layout->have_slck_mck)
0111         mask |= AT91_PMC_CSSMCK_MCK;
0112
0113     if (prog->mux_table)
0114         pckr = clk_mux_index_to_val(prog->mux_table, 0, index);
0115
0116     if (index > layout->css_mask) {
0117         if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck)
0118             return -EINVAL;
0119
0120         pckr |= AT91_PMC_CSSMCK_MCK;
0121     }
0122
0123     regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);
0124
0125     return 0;
0126 }
0127
0128 static u8 clk_programmable_get_parent(struct clk_hw *hw)
0129 {
0130     struct clk_programmable *prog = to_clk_programmable(hw);
0131     const struct clk_programmable_layout *layout = prog->layout;
0132     unsigned int pckr;
0133     u8 ret;
0134
0135     regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
0136
0137     ret = pckr & layout->css_mask;
0138
0139     if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret)
0140         ret = PROG_MAX_RM9200_CSS + 1;
0141
0142     if (prog->mux_table)
0143         ret = clk_mux_val_to_index(&prog->hw, prog->mux_table, 0, ret);
0144
0145     return ret;
0146 }
0147
0148 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
0149                      unsigned long parent_rate)
0150 {
0151     struct clk_programmable *prog = to_clk_programmable(hw);
0152     const struct clk_programmable_layout *layout = prog->layout;
0153     unsigned long div = parent_rate / rate;
0154     int shift = 0;
0155
0156     if (!div)
0157         return -EINVAL;
0158
0159     if (layout->is_pres_direct) {
0160         shift = div - 1;
0161
0162         if (shift > layout->pres_mask)
0163             return -EINVAL;
0164     } else {
0165         shift = fls(div) - 1;
0166
0167         if (div != (1 << shift))
0168             return -EINVAL;
0169
0170         if (shift >= layout->pres_mask)
0171             return -EINVAL;
0172     }
0173
0174     regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id),
0175                layout->pres_mask << layout->pres_shift,
0176                shift << layout->pres_shift);
0177
0178     return 0;
0179 }
0180
0181 static int clk_programmable_save_context(struct clk_hw *hw)
0182 {
0183     struct clk_programmable *prog = to_clk_programmable(hw);
0184     struct clk_hw *parent_hw = clk_hw_get_parent(hw);
0185
0186     prog->pms.parent = clk_programmable_get_parent(hw);
0187     prog->pms.parent_rate = clk_hw_get_rate(parent_hw);
0188     prog->pms.rate = clk_programmable_recalc_rate(hw, prog->pms.parent_rate);
0189
0190     return 0;
0191 }
0192
0193 static void clk_programmable_restore_context(struct clk_hw *hw)
0194 {
0195     struct clk_programmable *prog = to_clk_programmable(hw);
0196     int ret;
0197
0198     ret = clk_programmable_set_parent(hw, prog->pms.parent);
0199     if (ret)
0200         return;
0201
0202     clk_programmable_set_rate(hw, prog->pms.rate, prog->pms.parent_rate);
0203 }
0204
0205 static const struct clk_ops programmable_ops = {
0206     .recalc_rate = clk_programmable_recalc_rate,
0207     .determine_rate = clk_programmable_determine_rate,
0208     .get_parent = clk_programmable_get_parent,
0209     .set_parent = clk_programmable_set_parent,
0210     .set_rate = clk_programmable_set_rate,
0211     .save_context = clk_programmable_save_context,
0212     .restore_context = clk_programmable_restore_context,
0213 };
0214
0215 struct clk_hw * __init
0216 at91_clk_register_programmable(struct regmap *regmap,
0217                    const char *name, const char **parent_names,
0218                    u8 num_parents, u8 id,
0219                    const struct clk_programmable_layout *layout,
0220                    u32 *mux_table)
0221 {
0222     struct clk_programmable *prog;
0223     struct clk_hw *hw;
0224     struct clk_init_data init;
0225     int ret;
0226
0227     if (id > PROG_ID_MAX)
0228         return ERR_PTR(-EINVAL);
0229
0230     prog = kzalloc(sizeof(*prog), GFP_KERNEL);
0231     if (!prog)
0232         return ERR_PTR(-ENOMEM);
0233
0234     init.name = name;
0235     init.ops = &programmable_ops;
0236     init.parent_names = parent_names;
0237     init.num_parents = num_parents;
0238     init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
0239
0240     prog->id = id;
0241     prog->layout = layout;
0242     prog->hw.init = &init;
0243     prog->regmap = regmap;
0244     prog->mux_table = mux_table;
0245
0246     hw = &prog->hw;
0247     ret = clk_hw_register(NULL, &prog->hw);
0248     if (ret) {
0249         kfree(prog);
0250         hw = ERR_PTR(ret);
0251     }
0252
0253     return hw;
0254 }
0255
0256 const struct clk_programmable_layout at91rm9200_programmable_layout = {
0257     .pres_mask = 0x7,
0258     .pres_shift = 2,
0259     .css_mask = 0x3,
0260     .have_slck_mck = 0,
0261     .is_pres_direct = 0,
0262 };
0263
0264 const struct clk_programmable_layout at91sam9g45_programmable_layout = {
0265     .pres_mask = 0x7,
0266     .pres_shift = 2,
0267     .css_mask = 0x3,
0268     .have_slck_mck = 1,
0269     .is_pres_direct = 0,
0270 };
0271
0272 const struct clk_programmable_layout at91sam9x5_programmable_layout = {
0273     .pres_mask = 0x7,
0274     .pres_shift = 4,
0275     .css_mask = 0x7,
0276     .have_slck_mck = 0,
0277     .is_pres_direct = 0,
0278 };