drivers/clk/at91/clk-programmable.c

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