Branch data     Line data    Source code

       1                 :            : // SPDX-License-Identifier: GPL-2.0
       2                 :            : /*
       3                 :            :  * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
       4                 :            :  */
       5                 :            : 
       6                 :            : #include <linux/bitops.h>
       7                 :            : #include <linux/clk-provider.h>
       8                 :            : #include <linux/err.h>
       9                 :            : #include <linux/export.h>
      10                 :            : #include <linux/io.h>
      11                 :            : #include <linux/kernel.h>
      12                 :            : #include <linux/of.h>
      13                 :            : #include <linux/slab.h>
      14                 :            : 
      15                 :            : static inline u32 clk_mult_readl(struct clk_multiplier *mult)
      16                 :            : {
      17                 :          0 :         if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
      18                 :          0 :                 return ioread32be(mult->reg);
      19                 :            : 
      20                 :          0 :         return readl(mult->reg);
      21                 :            : }
      22                 :            : 
      23                 :            : static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
      24                 :            : {
      25                 :          0 :         if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
      26                 :          0 :                 iowrite32be(val, mult->reg);
      27                 :            :         else
      28                 :          0 :                 writel(val, mult->reg);
      29                 :            : }
      30                 :            : 
      31                 :            : static unsigned long __get_mult(struct clk_multiplier *mult,
      32                 :            :                                 unsigned long rate,
      33                 :            :                                 unsigned long parent_rate)
      34                 :            : {
      35                 :          0 :         if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
      36                 :          0 :                 return DIV_ROUND_CLOSEST(rate, parent_rate);
      37                 :            : 
      38                 :          0 :         return rate / parent_rate;
      39                 :            : }
      40                 :            : 
      41                 :          0 : static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
      42                 :            :                                                 unsigned long parent_rate)
      43                 :            : {
      44                 :            :         struct clk_multiplier *mult = to_clk_multiplier(hw);
      45                 :            :         unsigned long val;
      46                 :            : 
      47                 :          0 :         val = clk_mult_readl(mult) >> mult->shift;
      48                 :          0 :         val &= GENMASK(mult->width - 1, 0);
      49                 :            : 
      50                 :          0 :         if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
      51                 :            :                 val = 1;
      52                 :            : 
      53                 :          0 :         return parent_rate * val;
      54                 :            : }
      55                 :            : 
      56                 :            : static bool __is_best_rate(unsigned long rate, unsigned long new,
      57                 :            :                            unsigned long best, unsigned long flags)
      58                 :            : {
      59                 :          0 :         if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
      60                 :          0 :                 return abs(rate - new) < abs(rate - best);
      61                 :            : 
      62                 :          0 :         return new >= rate && new < best;
      63                 :            : }
      64                 :            : 
      65                 :          0 : static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
      66                 :            :                                 unsigned long *best_parent_rate,
      67                 :            :                                 u8 width, unsigned long flags)
      68                 :            : {
      69                 :            :         struct clk_multiplier *mult = to_clk_multiplier(hw);
      70                 :          0 :         unsigned long orig_parent_rate = *best_parent_rate;
      71                 :            :         unsigned long parent_rate, current_rate, best_rate = ~0;
      72                 :            :         unsigned int i, bestmult = 0;
      73                 :          0 :         unsigned int maxmult = (1 << width) - 1;
      74                 :            : 
      75                 :          0 :         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
      76                 :          0 :                 bestmult = rate / orig_parent_rate;
      77                 :            : 
      78                 :            :                 /* Make sure we don't end up with a 0 multiplier */
      79                 :          0 :                 if ((bestmult == 0) &&
      80                 :          0 :                     !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
      81                 :            :                         bestmult = 1;
      82                 :            : 
      83                 :            :                 /* Make sure we don't overflow the multiplier */
      84                 :          0 :                 if (bestmult > maxmult)
      85                 :            :                         bestmult = maxmult;
      86                 :            : 
      87                 :          0 :                 return bestmult;
      88                 :            :         }
      89                 :            : 
      90                 :          0 :         for (i = 1; i < maxmult; i++) {
      91                 :          0 :                 if (rate == orig_parent_rate * i) {
      92                 :            :                         /*
      93                 :            :                          * This is the best case for us if we have a
      94                 :            :                          * perfect match without changing the parent
      95                 :            :                          * rate.
      96                 :            :                          */
      97                 :          0 :                         *best_parent_rate = orig_parent_rate;
      98                 :          0 :                         return i;
      99                 :            :                 }
     100                 :            : 
     101                 :          0 :                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
     102                 :            :                                                 rate / i);
     103                 :          0 :                 current_rate = parent_rate * i;
     104                 :            : 
     105                 :          0 :                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
     106                 :            :                         bestmult = i;
     107                 :            :                         best_rate = current_rate;
     108                 :          0 :                         *best_parent_rate = parent_rate;
     109                 :            :                 }
     110                 :            :         }
     111                 :            : 
     112                 :          0 :         return bestmult;
     113                 :            : }
     114                 :            : 
     115                 :          0 : static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
     116                 :            :                                   unsigned long *parent_rate)
     117                 :            : {
     118                 :            :         struct clk_multiplier *mult = to_clk_multiplier(hw);
     119                 :          0 :         unsigned long factor = __bestmult(hw, rate, parent_rate,
     120                 :          0 :                                           mult->width, mult->flags);
     121                 :            : 
     122                 :          0 :         return *parent_rate * factor;
     123                 :            : }
     124                 :            : 
     125                 :          0 : static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
     126                 :            :                                unsigned long parent_rate)
     127                 :            : {
     128                 :            :         struct clk_multiplier *mult = to_clk_multiplier(hw);
     129                 :            :         unsigned long factor = __get_mult(mult, rate, parent_rate);
     130                 :            :         unsigned long flags = 0;
     131                 :            :         unsigned long val;
     132                 :            : 
     133                 :          0 :         if (mult->lock)
     134                 :          0 :                 spin_lock_irqsave(mult->lock, flags);
     135                 :            :         else
     136                 :            :                 __acquire(mult->lock);
     137                 :            : 
     138                 :            :         val = clk_mult_readl(mult);
     139                 :          0 :         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
     140                 :          0 :         val |= factor << mult->shift;
     141                 :            :         clk_mult_writel(mult, val);
     142                 :            : 
     143                 :          0 :         if (mult->lock)
     144                 :            :                 spin_unlock_irqrestore(mult->lock, flags);
     145                 :            :         else
     146                 :            :                 __release(mult->lock);
     147                 :            : 
     148                 :          0 :         return 0;
     149                 :            : }
     150                 :            : 
     151                 :            : const struct clk_ops clk_multiplier_ops = {
     152                 :            :         .recalc_rate    = clk_multiplier_recalc_rate,
     153                 :            :         .round_rate     = clk_multiplier_round_rate,
     154                 :            :         .set_rate       = clk_multiplier_set_rate,
     155                 :            : };
     156                 :            : EXPORT_SYMBOL_GPL(clk_multiplier_ops);