drivers/clk/at91/clk-programmable.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2013 Boris BREZILLON <[email protected]>
   4  */
   5
   6 #include <linux/clk-provider.h>
   7 #include <linux/clkdev.h>
   8 #include <linux/clk/at91_pmc.h>
   9 #include <linux/of.h>
  10 #include <linux/mfd/syscon.h>
  11 #include <linux/regmap.h>
  12
  13 #include "pmc.h"
  14
  15 #define PROG_ID_MAX             7
  16
  17 #define PROG_STATUS_MASK(id)    (1 << ((id) + 8))
  18 #define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask)
  19 #define PROG_MAX_RM9200_CSS     3
  20
  21 struct clk_programmable {
  22         struct clk_hw hw;
  23         struct regmap *regmap;
  24         u8 id;
  25         const struct clk_programmable_layout *layout;
  26 };
  27
  28 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
  29
  30 static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
  31                                                   unsigned long parent_rate)
  32 {
  33         struct clk_programmable *prog = to_clk_programmable(hw);
  34         const struct clk_programmable_layout *layout = prog->layout;
  35         unsigned int pckr;
  36         unsigned long rate;
  37
  38         regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
  39
  40         if (layout->is_pres_direct)
  41                 rate = parent_rate / (PROG_PRES(layout, pckr) + 1);
  42         else
  43                 rate = parent_rate >> PROG_PRES(layout, pckr);
  44
  45         return rate;
  46 }
  47
  48 static int clk_programmable_determine_rate(struct clk_hw *hw,
  49                                            struct clk_rate_request *req)
  50 {
  51         struct clk_programmable *prog = to_clk_programmable(hw);
  52         const struct clk_programmable_layout *layout = prog->layout;
  53         struct clk_hw *parent;
  54         long best_rate = -EINVAL;
  55         unsigned long parent_rate;
  56         unsigned long tmp_rate = 0;
  57         int shift;
  58         int i;
  59
  60         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  61                 parent = clk_hw_get_parent_by_index(hw, i);
  62                 if (!parent)
  63                         continue;
  64
  65                 parent_rate = clk_hw_get_rate(parent);
  66                 if (layout->is_pres_direct) {
  67                         for (shift = 0; shift <= layout->pres_mask; shift++) {
  68                                 tmp_rate = parent_rate / (shift + 1);
  69                                 if (tmp_rate <= req->rate)
  70                                         break;
  71                         }
  72                 } else {
  73                         for (shift = 0; shift < layout->pres_mask; shift++) {
  74                                 tmp_rate = parent_rate >> shift;
  75                                 if (tmp_rate <= req->rate)
  76                                         break;
  77                         }
  78                 }
  79
  80                 if (tmp_rate > req->rate)
  81                         continue;
  82
  83                 if (best_rate < 0 ||
  84                     (req->rate - tmp_rate) < (req->rate - best_rate)) {
  85                         best_rate = tmp_rate;
  86                         req->best_parent_rate = parent_rate;
  87                         req->best_parent_hw = parent;
  88                 }
  89
  90                 if (!best_rate)
  91                         break;
  92         }
  93
  94         if (best_rate < 0)
  95                 return best_rate;
  96
  97         req->rate = best_rate;
  98         return 0;
  99 }
 100
 101 static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)
 102 {
 103         struct clk_programmable *prog = to_clk_programmable(hw);
 104         const struct clk_programmable_layout *layout = prog->layout;
 105         unsigned int mask = layout->css_mask;
 106         unsigned int pckr = index;
 107
 108         if (layout->have_slck_mck)
 109                 mask |= AT91_PMC_CSSMCK_MCK;
 110
 111         if (index > layout->css_mask) {
 112                 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck)
 113                         return -EINVAL;
 114
 115                 pckr |= AT91_PMC_CSSMCK_MCK;
 116         }
 117
 118         regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);
 119
 120         return 0;
 121 }
 122
 123 static u8 clk_programmable_get_parent(struct clk_hw *hw)
 124 {
 125         struct clk_programmable *prog = to_clk_programmable(hw);
 126         const struct clk_programmable_layout *layout = prog->layout;
 127         unsigned int pckr;
 128         u8 ret;
 129
 130         regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
 131
 132         ret = pckr & layout->css_mask;
 133
 134         if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret)
 135                 ret = PROG_MAX_RM9200_CSS + 1;
 136
 137         return ret;
 138 }
 139
 140 static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
 141                                      unsigned long parent_rate)
 142 {
 143         struct clk_programmable *prog = to_clk_programmable(hw);
 144         const struct clk_programmable_layout *layout = prog->layout;
 145         unsigned long div = parent_rate / rate;
 146         int shift = 0;
 147
 148         if (!div)
 149                 return -EINVAL;
 150
 151         if (layout->is_pres_direct) {
 152                 shift = div - 1;
 153
 154                 if (shift > layout->pres_mask)
 155                         return -EINVAL;
 156         } else {
 157                 shift = fls(div) - 1;
 158
 159                 if (div != (1 << shift))
 160                         return -EINVAL;
 161
 162                 if (shift >= layout->pres_mask)
 163                         return -EINVAL;
 164         }
 165
 166         regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id),
 167                            layout->pres_mask << layout->pres_shift,
 168                            shift << layout->pres_shift);
 169
 170         return 0;
 171 }
 172
 173 static const struct clk_ops programmable_ops = {
 174         .recalc_rate = clk_programmable_recalc_rate,
 175         .determine_rate = clk_programmable_determine_rate,
 176         .get_parent = clk_programmable_get_parent,
 177         .set_parent = clk_programmable_set_parent,
 178         .set_rate = clk_programmable_set_rate,
 179 };
 180
 181 struct clk_hw * __init
 182 at91_clk_register_programmable(struct regmap *regmap,
 183                                const char *name, const char **parent_names,
 184                                u8 num_parents, u8 id,
 185                                const struct clk_programmable_layout *layout)
 186 {
 187         struct clk_programmable *prog;
 188         struct clk_hw *hw;
 189         struct clk_init_data init;
 190         int ret;
 191
 192         if (id > PROG_ID_MAX)
 193                 return ERR_PTR(-EINVAL);
 194
 195         prog = kzalloc(sizeof(*prog), GFP_KERNEL);
 196         if (!prog)
 197                 return ERR_PTR(-ENOMEM);
 198
 199         init.name = name;
 200         init.ops = &programmable_ops;
 201         init.parent_names = parent_names;
 202         init.num_parents = num_parents;
 203         init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 204
 205         prog->id = id;
 206         prog->layout = layout;
 207         prog->hw.init = &init;
 208         prog->regmap = regmap;
 209
 210         hw = &prog->hw;
 211         ret = clk_hw_register(NULL, &prog->hw);
 212         if (ret) {
 213                 kfree(prog);
 214                 hw = ERR_PTR(ret);
 215         } else {
 216                 pmc_register_pck(id);
 217         }
 218
 219         return hw;
 220 }
 221
 222 const struct clk_programmable_layout at91rm9200_programmable_layout = {
 223         .pres_mask = 0x7,
 224         .pres_shift = 2,
 225         .css_mask = 0x3,
 226         .have_slck_mck = 0,
 227         .is_pres_direct = 0,
 228 };
 229
 230 const struct clk_programmable_layout at91sam9g45_programmable_layout = {
 231         .pres_mask = 0x7,
 232         .pres_shift = 2,
 233         .css_mask = 0x3,
 234         .have_slck_mck = 1,
 235         .is_pres_direct = 0,
 236 };
 237
 238 const struct clk_programmable_layout at91sam9x5_programmable_layout = {
 239         .pres_mask = 0x7,
 240         .pres_shift = 4,
 241         .css_mask = 0x7,
 242         .have_slck_mck = 0,
 243         .is_pres_direct = 0,
 244 };