282 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>4 */5 6#include <linux/clk-provider.h>7#include <linux/clkdev.h>8#include <linux/clk/at91_pmc.h>9#include <linux/of.h>10#include <linux/mfd/syscon.h>11#include <linux/regmap.h>12 13#include "pmc.h"14 15#define PROG_ID_MAX 716 17#define PROG_STATUS_MASK(id) (1 << ((id) + 8))18#define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask)19#define PROG_MAX_RM9200_CSS 320 21struct clk_programmable {22 struct clk_hw hw;23 struct regmap *regmap;24 u32 *mux_table;25 u8 id;26 const struct clk_programmable_layout *layout;27 struct at91_clk_pms pms;28};29 30#define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)31 32static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,33 unsigned long parent_rate)34{35 struct clk_programmable *prog = to_clk_programmable(hw);36 const struct clk_programmable_layout *layout = prog->layout;37 unsigned int pckr;38 unsigned long rate;39 40 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);41 42 if (layout->is_pres_direct)43 rate = parent_rate / (PROG_PRES(layout, pckr) + 1);44 else45 rate = parent_rate >> PROG_PRES(layout, pckr);46 47 return rate;48}49 50static int clk_programmable_determine_rate(struct clk_hw *hw,51 struct clk_rate_request *req)52{53 struct clk_programmable *prog = to_clk_programmable(hw);54 const struct clk_programmable_layout *layout = prog->layout;55 struct clk_hw *parent;56 long best_rate = -EINVAL;57 unsigned long parent_rate;58 unsigned long tmp_rate = 0;59 int shift;60 int i;61 62 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {63 parent = clk_hw_get_parent_by_index(hw, i);64 if (!parent)65 continue;66 67 parent_rate = clk_hw_get_rate(parent);68 if (layout->is_pres_direct) {69 for (shift = 0; shift <= layout->pres_mask; shift++) {70 tmp_rate = parent_rate / (shift + 1);71 if (tmp_rate <= req->rate)72 break;73 }74 } else {75 for (shift = 0; shift < layout->pres_mask; shift++) {76 tmp_rate = parent_rate >> shift;77 if (tmp_rate <= req->rate)78 break;79 }80 }81 82 if (tmp_rate > req->rate)83 continue;84 85 if (best_rate < 0 ||86 (req->rate - tmp_rate) < (req->rate - best_rate)) {87 best_rate = tmp_rate;88 req->best_parent_rate = parent_rate;89 req->best_parent_hw = parent;90 }91 92 if (!best_rate)93 break;94 }95 96 if (best_rate < 0)97 return best_rate;98 99 req->rate = best_rate;100 return 0;101}102 103static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)104{105 struct clk_programmable *prog = to_clk_programmable(hw);106 const struct clk_programmable_layout *layout = prog->layout;107 unsigned int mask = layout->css_mask;108 unsigned int pckr = index;109 110 if (layout->have_slck_mck)111 mask |= AT91_PMC_CSSMCK_MCK;112 113 if (prog->mux_table)114 pckr = clk_mux_index_to_val(prog->mux_table, 0, index);115 116 if (index > layout->css_mask) {117 if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck)118 return -EINVAL;119 120 pckr |= AT91_PMC_CSSMCK_MCK;121 }122 123 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);124 125 return 0;126}127 128static u8 clk_programmable_get_parent(struct clk_hw *hw)129{130 struct clk_programmable *prog = to_clk_programmable(hw);131 const struct clk_programmable_layout *layout = prog->layout;132 unsigned int pckr;133 u8 ret;134 135 regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);136 137 ret = pckr & layout->css_mask;138 139 if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret)140 ret = PROG_MAX_RM9200_CSS + 1;141 142 if (prog->mux_table)143 ret = clk_mux_val_to_index(&prog->hw, prog->mux_table, 0, ret);144 145 return ret;146}147 148static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,149 unsigned long parent_rate)150{151 struct clk_programmable *prog = to_clk_programmable(hw);152 const struct clk_programmable_layout *layout = prog->layout;153 unsigned long div = parent_rate / rate;154 int shift = 0;155 156 if (!div)157 return -EINVAL;158 159 if (layout->is_pres_direct) {160 shift = div - 1;161 162 if (shift > layout->pres_mask)163 return -EINVAL;164 } else {165 shift = fls(div) - 1;166 167 if (div != (1 << shift))168 return -EINVAL;169 170 if (shift >= layout->pres_mask)171 return -EINVAL;172 }173 174 regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id),175 layout->pres_mask << layout->pres_shift,176 shift << layout->pres_shift);177 178 return 0;179}180 181static int clk_programmable_save_context(struct clk_hw *hw)182{183 struct clk_programmable *prog = to_clk_programmable(hw);184 struct clk_hw *parent_hw = clk_hw_get_parent(hw);185 186 prog->pms.parent = clk_programmable_get_parent(hw);187 prog->pms.parent_rate = clk_hw_get_rate(parent_hw);188 prog->pms.rate = clk_programmable_recalc_rate(hw, prog->pms.parent_rate);189 190 return 0;191}192 193static void clk_programmable_restore_context(struct clk_hw *hw)194{195 struct clk_programmable *prog = to_clk_programmable(hw);196 int ret;197 198 ret = clk_programmable_set_parent(hw, prog->pms.parent);199 if (ret)200 return;201 202 clk_programmable_set_rate(hw, prog->pms.rate, prog->pms.parent_rate);203}204 205static const struct clk_ops programmable_ops = {206 .recalc_rate = clk_programmable_recalc_rate,207 .determine_rate = clk_programmable_determine_rate,208 .get_parent = clk_programmable_get_parent,209 .set_parent = clk_programmable_set_parent,210 .set_rate = clk_programmable_set_rate,211 .save_context = clk_programmable_save_context,212 .restore_context = clk_programmable_restore_context,213};214 215struct clk_hw * __init216at91_clk_register_programmable(struct regmap *regmap,217 const char *name, const char **parent_names,218 struct clk_hw **parent_hws, u8 num_parents, u8 id,219 const struct clk_programmable_layout *layout,220 u32 *mux_table)221{222 struct clk_programmable *prog;223 struct clk_hw *hw;224 struct clk_init_data init = {};225 int ret;226 227 if (id > PROG_ID_MAX || !(parent_names || parent_hws))228 return ERR_PTR(-EINVAL);229 230 prog = kzalloc(sizeof(*prog), GFP_KERNEL);231 if (!prog)232 return ERR_PTR(-ENOMEM);233 234 init.name = name;235 init.ops = &programmable_ops;236 if (parent_hws)237 init.parent_hws = (const struct clk_hw **)parent_hws;238 else239 init.parent_names = parent_names;240 init.num_parents = num_parents;241 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;242 243 prog->id = id;244 prog->layout = layout;245 prog->hw.init = &init;246 prog->regmap = regmap;247 prog->mux_table = mux_table;248 249 hw = &prog->hw;250 ret = clk_hw_register(NULL, &prog->hw);251 if (ret) {252 kfree(prog);253 hw = ERR_PTR(ret);254 }255 256 return hw;257}258 259const struct clk_programmable_layout at91rm9200_programmable_layout = {260 .pres_mask = 0x7,261 .pres_shift = 2,262 .css_mask = 0x3,263 .have_slck_mck = 0,264 .is_pres_direct = 0,265};266 267const struct clk_programmable_layout at91sam9g45_programmable_layout = {268 .pres_mask = 0x7,269 .pres_shift = 2,270 .css_mask = 0x3,271 .have_slck_mck = 1,272 .is_pres_direct = 0,273};274 275const struct clk_programmable_layout at91sam9x5_programmable_layout = {276 .pres_mask = 0x7,277 .pres_shift = 4,278 .css_mask = 0x7,279 .have_slck_mck = 0,280 .is_pres_direct = 0,281};282