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