LXR linux/drivers/clk/sunxi-ng/ccu

   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_nk.h"
  15
  16struct _ccu_nk {
  17        unsigned long   n, min_n, max_n;
  18        unsigned long   k, min_k, max_k;
  19};
  20
  21static void ccu_nk_find_best(unsigned long parent, unsigned long rate,
  22                             struct _ccu_nk *nk)
  23{
  24        unsigned long best_rate = 0;
  25        unsigned int best_k = 0, best_n = 0;
  26        unsigned int _k, _n;
  27
  28        for (_k = nk->min_k; _k <= nk->max_k; _k++) {
  29                for (_n = nk->min_n; _n <= nk->max_n; _n++) {
  30                        unsigned long tmp_rate = parent * _n * _k;
  31
  32                        if (tmp_rate > rate)
  33                                continue;
  34
  35                        if ((rate - tmp_rate) < (rate - best_rate)) {
  36                                best_rate = tmp_rate;
  37                                best_k = _k;
  38                                best_n = _n;
  39                        }
  40                }
  41        }
  42
  43        nk->k = best_k;
  44        nk->n = best_n;
  45}
  46
  47static void ccu_nk_disable(struct clk_hw *hw)
  48{
  49        struct ccu_nk *nk = hw_to_ccu_nk(hw);
  50
  51        return ccu_gate_helper_disable(&nk->common, nk->enable);
  52}
  53
  54static int ccu_nk_enable(struct clk_hw *hw)
  55{
  56        struct ccu_nk *nk = hw_to_ccu_nk(hw);
  57
  58        return ccu_gate_helper_enable(&nk->common, nk->enable);
  59}
  60
  61static int ccu_nk_is_enabled(struct clk_hw *hw)
  62{
  63        struct ccu_nk *nk = hw_to_ccu_nk(hw);
  64
  65        return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
  66}
  67
  68static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
  69                                        unsigned long parent_rate)
  70{
  71        struct ccu_nk *nk = hw_to_ccu_nk(hw);
  72        unsigned long rate, n, k;
  73        u32 reg;
  74
  75        reg = readl(nk->common.base + nk->common.reg);
  76
  77        n = reg >> nk->n.shift;
  78        n &= (1 << nk->n.width) - 1;
  79        n += nk->n.offset;
  80        if (!n)
  81                n++;
  82
  83        k = reg >> nk->k.shift;
  84        k &= (1 << nk->k.width) - 1;
  85        k += nk->k.offset;
  86        if (!k)
  87                k++;
  88
  89        rate = parent_rate * n * k;
  90        if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  91                rate /= nk->fixed_post_div;
  92
  93        return rate;
  94}
  95
  96static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
  97                              unsigned long *parent_rate)
  98{
  99        struct ccu_nk *nk = hw_to_ccu_nk(hw);
 100        struct _ccu_nk _nk;
 101
 102        if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 103                rate *= nk->fixed_post_div;
 104
 105        _nk.min_n = nk->n.min ?: 1;
 106        _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 107        _nk.min_k = nk->k.min ?: 1;
 108        _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 109
 110        ccu_nk_find_best(*parent_rate, rate, &_nk);
 111        rate = *parent_rate * _nk.n * _nk.k;
 112
 113        if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 114                rate = rate / nk->fixed_post_div;
 115
 116        return rate;
 117}
 118
 119static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
 120                           unsigned long parent_rate)
 121{
 122        struct ccu_nk *nk = hw_to_ccu_nk(hw);
 123        unsigned long flags;
 124        struct _ccu_nk _nk;
 125        u32 reg;
 126
 127        if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 128                rate = rate * nk->fixed_post_div;
 129
 130        _nk.min_n = nk->n.min ?: 1;
 131        _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 132        _nk.min_k = nk->k.min ?: 1;
 133        _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 134
 135        ccu_nk_find_best(parent_rate, rate, &_nk);
 136
 137        spin_lock_irqsave(nk->common.lock, flags);
 138
 139        reg = readl(nk->common.base + nk->common.reg);
 140        reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
 141        reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
 142
 143        reg |= (_nk.k - nk->k.offset) << nk->k.shift;
 144        reg |= (_nk.n - nk->n.offset) << nk->n.shift;
 145        writel(reg, nk->common.base + nk->common.reg);
 146
 147        spin_unlock_irqrestore(nk->common.lock, flags);
 148
 149        ccu_helper_wait_for_lock(&nk->common, nk->lock);
 150
 151        return 0;
 152}
 153
 154const struct clk_ops ccu_nk_ops = {
 155        .disable        = ccu_nk_disable,
 156        .enable         = ccu_nk_enable,
 157        .is_enabled     = ccu_nk_is_enabled,
 158
 159        .recalc_rate    = ccu_nk_recalc_rate,
 160        .round_rate     = ccu_nk_round_rate,
 161        .set_rate       = ccu_nk_set_rate,
 162};
 163