143 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 SMD_DIV_SHIFT 816#define SMD_MAX_DIV 0xf17 18struct at91sam9x5_clk_smd {19 struct clk_hw hw;20 struct regmap *regmap;21};22 23#define to_at91sam9x5_clk_smd(hw) \24 container_of(hw, struct at91sam9x5_clk_smd, hw)25 26static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,27 unsigned long parent_rate)28{29 struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);30 unsigned int smdr;31 u8 smddiv;32 33 regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);34 smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;35 36 return parent_rate / (smddiv + 1);37}38 39static int at91sam9x5_clk_smd_determine_rate(struct clk_hw *hw,40 struct clk_rate_request *req)41{42 unsigned long div;43 unsigned long bestrate;44 unsigned long tmp;45 46 if (req->rate >= req->best_parent_rate) {47 req->rate = req->best_parent_rate;48 return 0;49 }50 51 div = req->best_parent_rate / req->rate;52 if (div > SMD_MAX_DIV) {53 req->rate = req->best_parent_rate / (SMD_MAX_DIV + 1);54 return 0;55 }56 57 bestrate = req->best_parent_rate / div;58 tmp = req->best_parent_rate / (div + 1);59 if (bestrate - req->rate > req->rate - tmp)60 bestrate = tmp;61 62 req->rate = bestrate;63 return 0;64}65 66static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)67{68 struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);69 70 if (index > 1)71 return -EINVAL;72 73 regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,74 index ? AT91_PMC_SMDS : 0);75 76 return 0;77}78 79static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)80{81 struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);82 unsigned int smdr;83 84 regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);85 86 return smdr & AT91_PMC_SMDS;87}88 89static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,90 unsigned long parent_rate)91{92 struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);93 unsigned long div = parent_rate / rate;94 95 if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))96 return -EINVAL;97 98 regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,99 (div - 1) << SMD_DIV_SHIFT);100 101 return 0;102}103 104static const struct clk_ops at91sam9x5_smd_ops = {105 .recalc_rate = at91sam9x5_clk_smd_recalc_rate,106 .determine_rate = at91sam9x5_clk_smd_determine_rate,107 .get_parent = at91sam9x5_clk_smd_get_parent,108 .set_parent = at91sam9x5_clk_smd_set_parent,109 .set_rate = at91sam9x5_clk_smd_set_rate,110};111 112struct clk_hw * __init113at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,114 const char **parent_names, u8 num_parents)115{116 struct at91sam9x5_clk_smd *smd;117 struct clk_hw *hw;118 struct clk_init_data init;119 int ret;120 121 smd = kzalloc(sizeof(*smd), GFP_KERNEL);122 if (!smd)123 return ERR_PTR(-ENOMEM);124 125 init.name = name;126 init.ops = &at91sam9x5_smd_ops;127 init.parent_names = parent_names;128 init.num_parents = num_parents;129 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;130 131 smd->hw.init = &init;132 smd->regmap = regmap;133 134 hw = &smd->hw;135 ret = clk_hw_register(NULL, &smd->hw);136 if (ret) {137 kfree(smd);138 hw = ERR_PTR(ret);139 }140 141 return hw;142}143