1 /* 2 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 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 as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 * 9 */ 10 11 #include <linux/clk-provider.h> 12 #include <linux/clkdev.h> 13 #include <linux/clk/at91_pmc.h> 14 #include <linux/of.h> 15 #include <linux/mfd/syscon.h> 16 #include <linux/regmap.h> 17 18 #include "pmc.h" 19 20 #define SMD_DIV_SHIFT 8 21 #define SMD_MAX_DIV 0xf 22 23 struct at91sam9x5_clk_smd { 24 struct clk_hw hw; 25 struct regmap *regmap; 26 }; 27 28 #define to_at91sam9x5_clk_smd(hw) \ 29 container_of(hw, struct at91sam9x5_clk_smd, hw) 30 31 static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw, 32 unsigned long parent_rate) 33 { 34 struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw); 35 unsigned int smdr; 36 u8 smddiv; 37 38 regmap_read(smd->regmap, AT91_PMC_SMD, &smdr); 39 smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT; 40 41 return parent_rate / (smddiv + 1); 42 } 43 44 static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate, 45 unsigned long *parent_rate) 46 { 47 unsigned long div; 48 unsigned long bestrate; 49 unsigned long tmp; 50 51 if (rate >= *parent_rate) 52 return *parent_rate; 53 54 div = *parent_rate / rate; 55 if (div > SMD_MAX_DIV) 56 return *parent_rate / (SMD_MAX_DIV + 1); 57 58 bestrate = *parent_rate / div; 59 tmp = *parent_rate / (div + 1); 60 if (bestrate - rate > rate - tmp) 61 bestrate = tmp; 62 63 return bestrate; 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 .round_rate = at91sam9x5_clk_smd_round_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