drivers/clk/sunxi-ng/ccu_mux.c

   1 /*
   2  * Copyright (C) 2016 Maxime Ripard
   3  * Maxime Ripard <maxime.ripard@free-electrons.com>
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of
   8  * the License, or (at your option) any later version.
   9  */
  10
  11 #include <linux/clk-provider.h>
  12
  13 #include "ccu_gate.h"
  14 #include "ccu_mux.h"
  15
  16 void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
  17                                              struct ccu_mux_internal *cm,
  18                                              int parent_index,
  19                                              unsigned long *parent_rate)
  20 {
  21         u8 prediv = 1;
  22         u32 reg;
  23
  24         if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
  25               (common->features & CCU_FEATURE_VARIABLE_PREDIV)))
  26                 return;
  27
  28         reg = readl(common->base + common->reg);
  29         if (parent_index < 0) {
  30                 parent_index = reg >> cm->shift;
  31                 parent_index &= (1 << cm->width) - 1;
  32         }
  33
  34         if (common->features & CCU_FEATURE_FIXED_PREDIV)
  35                 if (parent_index == cm->fixed_prediv.index)
  36                         prediv = cm->fixed_prediv.div;
  37
  38         if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
  39                 if (parent_index == cm->variable_prediv.index) {
  40                         u8 div;
  41
  42                         div = reg >> cm->variable_prediv.shift;
  43                         div &= (1 << cm->variable_prediv.width) - 1;
  44                         prediv = div + 1;
  45                 }
  46
  47         *parent_rate = *parent_rate / prediv;
  48 }
  49
  50 int ccu_mux_helper_determine_rate(struct ccu_common *common,
  51                                   struct ccu_mux_internal *cm,
  52                                   struct clk_rate_request *req,
  53                                   unsigned long (*round)(struct ccu_mux_internal *,
  54                                                          unsigned long,
  55                                                          unsigned long,
  56                                                          void *),
  57                                   void *data)
  58 {
  59         unsigned long best_parent_rate = 0, best_rate = 0;
  60         struct clk_hw *best_parent, *hw = &common->hw;
  61         unsigned int i;
  62
  63         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  64                 unsigned long tmp_rate, parent_rate;
  65                 struct clk_hw *parent;
  66
  67                 parent = clk_hw_get_parent_by_index(hw, i);
  68                 if (!parent)
  69                         continue;
  70
  71                 parent_rate = clk_hw_get_rate(parent);
  72                 ccu_mux_helper_adjust_parent_for_prediv(common, cm, i,
  73                                                         &parent_rate);
  74
  75                 tmp_rate = round(cm, clk_hw_get_rate(parent), req->rate, data);
  76                 if (tmp_rate == req->rate) {
  77                         best_parent = parent;
  78                         best_parent_rate = parent_rate;
  79                         best_rate = tmp_rate;
  80                         goto out;
  81                 }
  82
  83                 if ((req->rate - tmp_rate) < (req->rate - best_rate)) {
  84                         best_rate = tmp_rate;
  85                         best_parent_rate = parent_rate;
  86                         best_parent = parent;
  87                 }
  88         }
  89
  90         if (best_rate == 0)
  91                 return -EINVAL;
  92
  93 out:
  94         req->best_parent_hw = best_parent;
  95         req->best_parent_rate = best_parent_rate;
  96         req->rate = best_rate;
  97         return 0;
  98 }
  99
 100 u8 ccu_mux_helper_get_parent(struct ccu_common *common,
 101                              struct ccu_mux_internal *cm)
 102 {
 103         u32 reg;
 104         u8 parent;
 105
 106         reg = readl(common->base + common->reg);
 107         parent = reg >> cm->shift;
 108         parent &= (1 << cm->width) - 1;
 109
 110         return parent;
 111 }
 112
 113 int ccu_mux_helper_set_parent(struct ccu_common *common,
 114                               struct ccu_mux_internal *cm,
 115                               u8 index)
 116 {
 117         unsigned long flags;
 118         u32 reg;
 119
 120         spin_lock_irqsave(common->lock, flags);
 121
 122         reg = readl(common->base + common->reg);
 123         reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift);
 124         writel(reg | (index << cm->shift), common->base + common->reg);
 125
 126         spin_unlock_irqrestore(common->lock, flags);
 127
 128         return 0;
 129 }
 130
 131 static void ccu_mux_disable(struct clk_hw *hw)
 132 {
 133         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 134
 135         return ccu_gate_helper_disable(&cm->common, cm->enable);
 136 }
 137
 138 static int ccu_mux_enable(struct clk_hw *hw)
 139 {
 140         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 141
 142         return ccu_gate_helper_enable(&cm->common, cm->enable);
 143 }
 144
 145 static int ccu_mux_is_enabled(struct clk_hw *hw)
 146 {
 147         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 148
 149         return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
 150 }
 151
 152 static u8 ccu_mux_get_parent(struct clk_hw *hw)
 153 {
 154         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 155
 156         return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
 157 }
 158
 159 static int ccu_mux_set_parent(struct clk_hw *hw, u8 index)
 160 {
 161         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 162
 163         return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
 164 }
 165
 166 static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
 167                                          unsigned long parent_rate)
 168 {
 169         struct ccu_mux *cm = hw_to_ccu_mux(hw);
 170
 171         ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
 172                                                 &parent_rate);
 173
 174         return parent_rate;
 175 }
 176
 177 const struct clk_ops ccu_mux_ops = {
 178         .disable        = ccu_mux_disable,
 179         .enable         = ccu_mux_enable,
 180         .is_enabled     = ccu_mux_is_enabled,
 181
 182         .get_parent     = ccu_mux_get_parent,
 183         .set_parent     = ccu_mux_set_parent,
 184
 185         .determine_rate = __clk_mux_determine_rate,
 186         .recalc_rate    = ccu_mux_recalc_rate,
 187 };