drivers/clk/at91/clk-smd.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
   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 SMD_DIV_SHIFT           8
  16 #define SMD_MAX_DIV             0xf
  17
  18 struct at91sam9x5_clk_smd {
  19         struct clk_hw hw;
  20         struct regmap *regmap;
  21 };
  22
  23 #define to_at91sam9x5_clk_smd(hw) \
  24         container_of(hw, struct at91sam9x5_clk_smd, hw)
  25
  26 static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
  27                                                     unsigned long parent_rate)
  28 {
  29         struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  30         unsigned int smdr;
  31         u8 smddiv;
  32
  33         regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
  34         smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;
  35
  36         return parent_rate / (smddiv + 1);
  37 }
  38
  39 static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate,
  40                                           unsigned long *parent_rate)
  41 {
  42         unsigned long div;
  43         unsigned long bestrate;
  44         unsigned long tmp;
  45
  46         if (rate >= *parent_rate)
  47                 return *parent_rate;
  48
  49         div = *parent_rate / rate;
  50         if (div > SMD_MAX_DIV)
  51                 return *parent_rate / (SMD_MAX_DIV + 1);
  52
  53         bestrate = *parent_rate / div;
  54         tmp = *parent_rate / (div + 1);
  55         if (bestrate - rate > rate - tmp)
  56                 bestrate = tmp;
  57
  58         return bestrate;
  59 }
  60
  61 static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
  62 {
  63         struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  64
  65         if (index > 1)
  66                 return -EINVAL;
  67
  68         regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,
  69                            index ? AT91_PMC_SMDS : 0);
  70
  71         return 0;
  72 }
  73
  74 static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
  75 {
  76         struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  77         unsigned int smdr;
  78
  79         regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
  80
  81         return smdr & AT91_PMC_SMDS;
  82 }
  83
  84 static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
  85                                        unsigned long parent_rate)
  86 {
  87         struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  88         unsigned long div = parent_rate / rate;
  89
  90         if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))
  91                 return -EINVAL;
  92
  93         regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,
  94                            (div - 1) << SMD_DIV_SHIFT);
  95
  96         return 0;
  97 }
  98
  99 static const struct clk_ops at91sam9x5_smd_ops = {
 100         .recalc_rate = at91sam9x5_clk_smd_recalc_rate,
 101         .round_rate = at91sam9x5_clk_smd_round_rate,
 102         .get_parent = at91sam9x5_clk_smd_get_parent,
 103         .set_parent = at91sam9x5_clk_smd_set_parent,
 104         .set_rate = at91sam9x5_clk_smd_set_rate,
 105 };
 106
 107 struct clk_hw * __init
 108 at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
 109                             const char **parent_names, u8 num_parents)
 110 {
 111         struct at91sam9x5_clk_smd *smd;
 112         struct clk_hw *hw;
 113         struct clk_init_data init;
 114         int ret;
 115
 116         smd = kzalloc(sizeof(*smd), GFP_KERNEL);
 117         if (!smd)
 118                 return ERR_PTR(-ENOMEM);
 119
 120         init.name = name;
 121         init.ops = &at91sam9x5_smd_ops;
 122         init.parent_names = parent_names;
 123         init.num_parents = num_parents;
 124         init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 125
 126         smd->hw.init = &init;
 127         smd->regmap = regmap;
 128
 129         hw = &smd->hw;
 130         ret = clk_hw_register(NULL, &smd->hw);
 131         if (ret) {
 132                 kfree(smd);
 133                 hw = ERR_PTR(ret);
 134         }
 135
 136         return hw;
 137 }