1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 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 8 16 #define SMD_MAX_DIV 0xf 17 18 struct 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 26 static 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 39 static 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 66 static 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 79 static 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 89 static 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 104 static 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 112 struct clk_hw * __init 113 at91sam9x5_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