drivers/clk/imx/clk-composite-8m.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright 2018 NXP
   4  */
   5
   6 #include <linux/errno.h>
   7 #include <linux/slab.h>
   8 #include <linux/clk-provider.h>
   9
  10 #include "clk.h"
  11
  12 #define PCG_PREDIV_SHIFT        16
  13 #define PCG_PREDIV_WIDTH        3
  14 #define PCG_PREDIV_MAX          8
  15
  16 #define PCG_DIV_SHIFT           0
  17 #define PCG_DIV_WIDTH           6
  18 #define PCG_DIV_MAX             64
  19
  20 #define PCG_PCS_SHIFT           24
  21 #define PCG_PCS_MASK            0x7
  22
  23 #define PCG_CGC_SHIFT           28
  24
  25 static unsigned long imx8m_clk_composite_divider_recalc_rate(struct clk_hw *hw,
  26                                                 unsigned long parent_rate)
  27 {
  28         struct clk_divider *divider = to_clk_divider(hw);
  29         unsigned long prediv_rate;
  30         unsigned int prediv_value;
  31         unsigned int div_value;
  32
  33         prediv_value = readl(divider->reg) >> divider->shift;
  34         prediv_value &= clk_div_mask(divider->width);
  35
  36         prediv_rate = divider_recalc_rate(hw, parent_rate, prediv_value,
  37                                                 NULL, divider->flags,
  38                                                 divider->width);
  39
  40         div_value = readl(divider->reg) >> PCG_DIV_SHIFT;
  41         div_value &= clk_div_mask(PCG_DIV_WIDTH);
  42
  43         return divider_recalc_rate(hw, prediv_rate, div_value, NULL,
  44                                    divider->flags, PCG_DIV_WIDTH);
  45 }
  46
  47 static int imx8m_clk_composite_compute_dividers(unsigned long rate,
  48                                                 unsigned long parent_rate,
  49                                                 int *prediv, int *postdiv)
  50 {
  51         int div1, div2;
  52         int error = INT_MAX;
  53         int ret = -EINVAL;
  54
  55         *prediv = 1;
  56         *postdiv = 1;
  57
  58         for (div1 = 1; div1 <= PCG_PREDIV_MAX; div1++) {
  59                 for (div2 = 1; div2 <= PCG_DIV_MAX; div2++) {
  60                         int new_error = ((parent_rate / div1) / div2) - rate;
  61
  62                         if (abs(new_error) < abs(error)) {
  63                                 *prediv = div1;
  64                                 *postdiv = div2;
  65                                 error = new_error;
  66                                 ret = 0;
  67                         }
  68                 }
  69         }
  70         return ret;
  71 }
  72
  73 static long imx8m_clk_composite_divider_round_rate(struct clk_hw *hw,
  74                                                 unsigned long rate,
  75                                                 unsigned long *prate)
  76 {
  77         int prediv_value;
  78         int div_value;
  79
  80         imx8m_clk_composite_compute_dividers(rate, *prate,
  81                                                 &prediv_value, &div_value);
  82         rate = DIV_ROUND_UP(*prate, prediv_value);
  83
  84         return DIV_ROUND_UP(rate, div_value);
  85
  86 }
  87
  88 static int imx8m_clk_composite_divider_set_rate(struct clk_hw *hw,
  89                                         unsigned long rate,
  90                                         unsigned long parent_rate)
  91 {
  92         struct clk_divider *divider = to_clk_divider(hw);
  93         unsigned long flags = 0;
  94         int prediv_value;
  95         int div_value;
  96         int ret;
  97         u32 val;
  98
  99         ret = imx8m_clk_composite_compute_dividers(rate, parent_rate,
 100                                                 &prediv_value, &div_value);
 101         if (ret)
 102                 return -EINVAL;
 103
 104         spin_lock_irqsave(divider->lock, flags);
 105
 106         val = readl(divider->reg);
 107         val &= ~((clk_div_mask(divider->width) << divider->shift) |
 108                         (clk_div_mask(PCG_DIV_WIDTH) << PCG_DIV_SHIFT));
 109
 110         val |= (u32)(prediv_value  - 1) << divider->shift;
 111         val |= (u32)(div_value - 1) << PCG_DIV_SHIFT;
 112         writel(val, divider->reg);
 113
 114         spin_unlock_irqrestore(divider->lock, flags);
 115
 116         return ret;
 117 }
 118
 119 static const struct clk_ops imx8m_clk_composite_divider_ops = {
 120         .recalc_rate = imx8m_clk_composite_divider_recalc_rate,
 121         .round_rate = imx8m_clk_composite_divider_round_rate,
 122         .set_rate = imx8m_clk_composite_divider_set_rate,
 123 };
 124
 125 struct clk *imx8m_clk_composite_flags(const char *name,
 126                                         const char * const *parent_names,
 127                                         int num_parents, void __iomem *reg,
 128                                         unsigned long flags)
 129 {
 130         struct clk_hw *hw = ERR_PTR(-ENOMEM), *mux_hw;
 131         struct clk_hw *div_hw, *gate_hw;
 132         struct clk_divider *div = NULL;
 133         struct clk_gate *gate = NULL;
 134         struct clk_mux *mux = NULL;
 135
 136         mux = kzalloc(sizeof(*mux), GFP_KERNEL);
 137         if (!mux)
 138                 goto fail;
 139
 140         mux_hw = &mux->hw;
 141         mux->reg = reg;
 142         mux->shift = PCG_PCS_SHIFT;
 143         mux->mask = PCG_PCS_MASK;
 144
 145         div = kzalloc(sizeof(*div), GFP_KERNEL);
 146         if (!div)
 147                 goto fail;
 148
 149         div_hw = &div->hw;
 150         div->reg = reg;
 151         div->shift = PCG_PREDIV_SHIFT;
 152         div->width = PCG_PREDIV_WIDTH;
 153         div->lock = &imx_ccm_lock;
 154         div->flags = CLK_DIVIDER_ROUND_CLOSEST;
 155
 156         gate = kzalloc(sizeof(*gate), GFP_KERNEL);
 157         if (!gate)
 158                 goto fail;
 159
 160         gate_hw = &gate->hw;
 161         gate->reg = reg;
 162         gate->bit_idx = PCG_CGC_SHIFT;
 163
 164         hw = clk_hw_register_composite(NULL, name, parent_names, num_parents,
 165                         mux_hw, &clk_mux_ops, div_hw,
 166                         &imx8m_clk_composite_divider_ops,
 167                         gate_hw, &clk_gate_ops, flags);
 168         if (IS_ERR(hw))
 169                 goto fail;
 170
 171         return hw->clk;
 172
 173 fail:
 174         kfree(gate);
 175         kfree(div);
 176         kfree(mux);
 177         return ERR_CAST(hw);
 178 }