1 /* 2 * Copyright (C) 2013 Emilio López <emilio@elopez.com.ar> 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License version 2 as 6 * published by the Free Software Foundation. 7 * 8 * Adjustable factor-based clock implementation 9 */ 10 11 #include <linux/clk-provider.h> 12 #include <linux/module.h> 13 #include <linux/slab.h> 14 #include <linux/io.h> 15 #include <linux/err.h> 16 #include <linux/string.h> 17 18 #include <linux/delay.h> 19 20 #include "clk-factors.h" 21 22 /* 23 * DOC: basic adjustable factor-based clock that cannot gate 24 * 25 * Traits of this clock: 26 * prepare - clk_prepare only ensures that parents are prepared 27 * enable - clk_enable only ensures that parents are enabled 28 * rate - rate is adjustable. 29 * clk->rate = (parent->rate * N * (K + 1) >> P) / (M + 1) 30 * parent - fixed parent. No clk_set_parent support 31 */ 32 33 #define to_clk_factors(_hw) container_of(_hw, struct clk_factors, hw) 34 35 #define SETMASK(len, pos) (((1U << (len)) - 1) << (pos)) 36 #define CLRMASK(len, pos) (~(SETMASK(len, pos))) 37 #define FACTOR_GET(bit, len, reg) (((reg) & SETMASK(len, bit)) >> (bit)) 38 39 #define FACTOR_SET(bit, len, reg, val) \ 40 (((reg) & CLRMASK(len, bit)) | (val << (bit))) 41 42 static unsigned long clk_factors_recalc_rate(struct clk_hw *hw, 43 unsigned long parent_rate) 44 { 45 u8 n = 1, k = 0, p = 0, m = 0; 46 u32 reg; 47 unsigned long rate; 48 struct clk_factors *factors = to_clk_factors(hw); 49 struct clk_factors_config *config = factors->config; 50 51 /* Fetch the register value */ 52 reg = readl(factors->reg); 53 54 /* Get each individual factor if applicable */ 55 if (config->nwidth != SUNXI_FACTORS_NOT_APPLICABLE) 56 n = FACTOR_GET(config->nshift, config->nwidth, reg); 57 if (config->kwidth != SUNXI_FACTORS_NOT_APPLICABLE) 58 k = FACTOR_GET(config->kshift, config->kwidth, reg); 59 if (config->mwidth != SUNXI_FACTORS_NOT_APPLICABLE) 60 m = FACTOR_GET(config->mshift, config->mwidth, reg); 61 if (config->pwidth != SUNXI_FACTORS_NOT_APPLICABLE) 62 p = FACTOR_GET(config->pshift, config->pwidth, reg); 63 64 /* Calculate the rate */ 65 rate = (parent_rate * n * (k + 1) >> p) / (m + 1); 66 67 return rate; 68 } 69 70 static long clk_factors_round_rate(struct clk_hw *hw, unsigned long rate, 71 unsigned long *parent_rate) 72 { 73 struct clk_factors *factors = to_clk_factors(hw); 74 factors->get_factors((u32 *)&rate, (u32)*parent_rate, 75 NULL, NULL, NULL, NULL); 76 77 return rate; 78 } 79 80 static long clk_factors_determine_rate(struct clk_hw *hw, unsigned long rate, 81 unsigned long *best_parent_rate, 82 struct clk **best_parent_p) 83 { 84 struct clk *clk = hw->clk, *parent, *best_parent = NULL; 85 int i, num_parents; 86 unsigned long parent_rate, best = 0, child_rate, best_child_rate = 0; 87 88 /* find the parent that can help provide the fastest rate <= rate */ 89 num_parents = __clk_get_num_parents(clk); 90 for (i = 0; i < num_parents; i++) { 91 parent = clk_get_parent_by_index(clk, i); 92 if (!parent) 93 continue; 94 if (__clk_get_flags(clk) & CLK_SET_RATE_PARENT) 95 parent_rate = __clk_round_rate(parent, rate); 96 else 97 parent_rate = __clk_get_rate(parent); 98 99 child_rate = clk_factors_round_rate(hw, rate, &parent_rate); 100 101 if (child_rate <= rate && child_rate > best_child_rate) { 102 best_parent = parent; 103 best = parent_rate; 104 best_child_rate = child_rate; 105 } 106 } 107 108 if (best_parent) 109 *best_parent_p = best_parent; 110 *best_parent_rate = best; 111 112 return best_child_rate; 113 } 114 115 static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate, 116 unsigned long parent_rate) 117 { 118 u8 n = 0, k = 0, m = 0, p = 0; 119 u32 reg; 120 struct clk_factors *factors = to_clk_factors(hw); 121 struct clk_factors_config *config = factors->config; 122 unsigned long flags = 0; 123 124 factors->get_factors((u32 *)&rate, (u32)parent_rate, &n, &k, &m, &p); 125 126 if (factors->lock) 127 spin_lock_irqsave(factors->lock, flags); 128 129 /* Fetch the register value */ 130 reg = readl(factors->reg); 131 132 /* Set up the new factors - macros do not do anything if width is 0 */ 133 reg = FACTOR_SET(config->nshift, config->nwidth, reg, n); 134 reg = FACTOR_SET(config->kshift, config->kwidth, reg, k); 135 reg = FACTOR_SET(config->mshift, config->mwidth, reg, m); 136 reg = FACTOR_SET(config->pshift, config->pwidth, reg, p); 137 138 /* Apply them now */ 139 writel(reg, factors->reg); 140 141 /* delay 500us so pll stabilizes */ 142 __delay((rate >> 20) * 500 / 2); 143 144 if (factors->lock) 145 spin_unlock_irqrestore(factors->lock, flags); 146 147 return 0; 148 } 149 150 const struct clk_ops clk_factors_ops = { 151 .determine_rate = clk_factors_determine_rate, 152 .recalc_rate = clk_factors_recalc_rate, 153 .round_rate = clk_factors_round_rate, 154 .set_rate = clk_factors_set_rate, 155 }; 156