1 /* 2 * Copyright (C) 2016 Rafał Miłecki <rafal@milecki.pl> 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 version 2 as 6 * published by the Free Software Foundation. 7 */ 8 9 #include <linux/clk-provider.h> 10 #include <linux/err.h> 11 #include <linux/io.h> 12 #include <linux/mfd/syscon.h> 13 #include <linux/of.h> 14 #include <linux/of_address.h> 15 #include <linux/regmap.h> 16 #include <linux/slab.h> 17 18 #define PMU_XTAL_FREQ_RATIO 0x66c 19 #define XTAL_ALP_PER_4ILP 0x00001fff 20 #define XTAL_CTL_EN 0x80000000 21 #define PMU_SLOW_CLK_PERIOD 0x6dc 22 23 struct bcm53573_ilp { 24 struct clk_hw hw; 25 struct regmap *regmap; 26 }; 27 28 static int bcm53573_ilp_enable(struct clk_hw *hw) 29 { 30 struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); 31 32 regmap_write(ilp->regmap, PMU_SLOW_CLK_PERIOD, 0x10199); 33 regmap_write(ilp->regmap, 0x674, 0x10000); 34 35 return 0; 36 } 37 38 static void bcm53573_ilp_disable(struct clk_hw *hw) 39 { 40 struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); 41 42 regmap_write(ilp->regmap, PMU_SLOW_CLK_PERIOD, 0); 43 regmap_write(ilp->regmap, 0x674, 0); 44 } 45 46 static unsigned long bcm53573_ilp_recalc_rate(struct clk_hw *hw, 47 unsigned long parent_rate) 48 { 49 struct bcm53573_ilp *ilp = container_of(hw, struct bcm53573_ilp, hw); 50 struct regmap *regmap = ilp->regmap; 51 u32 last_val, cur_val; 52 int sum = 0, num = 0, loop_num = 0; 53 int avg; 54 55 /* Enable measurement */ 56 regmap_write(regmap, PMU_XTAL_FREQ_RATIO, XTAL_CTL_EN); 57 58 /* Read initial value */ 59 regmap_read(regmap, PMU_XTAL_FREQ_RATIO, &last_val); 60 last_val &= XTAL_ALP_PER_4ILP; 61 62 /* 63 * At minimum we should loop for a bit to let hardware do the 64 * measurement. This isn't very accurate however, so for a better 65 * precision lets try getting 20 different values for and use average. 66 */ 67 while (num < 20) { 68 regmap_read(regmap, PMU_XTAL_FREQ_RATIO, &cur_val); 69 cur_val &= XTAL_ALP_PER_4ILP; 70 71 if (cur_val != last_val) { 72 /* Got different value, use it */ 73 sum += cur_val; 74 num++; 75 loop_num = 0; 76 last_val = cur_val; 77 } else if (++loop_num > 5000) { 78 /* Same value over and over, give up */ 79 sum += cur_val; 80 num++; 81 break; 82 } 83 84 cpu_relax(); 85 } 86 87 /* Disable measurement to save power */ 88 regmap_write(regmap, PMU_XTAL_FREQ_RATIO, 0x0); 89 90 avg = sum / num; 91 92 return parent_rate * 4 / avg; 93 } 94 95 static const struct clk_ops bcm53573_ilp_clk_ops = { 96 .enable = bcm53573_ilp_enable, 97 .disable = bcm53573_ilp_disable, 98 .recalc_rate = bcm53573_ilp_recalc_rate, 99 }; 100 101 static void bcm53573_ilp_init(struct device_node *np) 102 { 103 struct bcm53573_ilp *ilp; 104 struct clk_init_data init = { }; 105 const char *parent_name; 106 int err; 107 108 ilp = kzalloc(sizeof(*ilp), GFP_KERNEL); 109 if (!ilp) 110 return; 111 112 parent_name = of_clk_get_parent_name(np, 0); 113 if (!parent_name) { 114 err = -ENOENT; 115 goto err_free_ilp; 116 } 117 118 ilp->regmap = syscon_node_to_regmap(of_get_parent(np)); 119 if (IS_ERR(ilp->regmap)) { 120 err = PTR_ERR(ilp->regmap); 121 goto err_free_ilp; 122 } 123 124 init.name = np->name; 125 init.ops = &bcm53573_ilp_clk_ops; 126 init.parent_names = &parent_name; 127 init.num_parents = 1; 128 129 ilp->hw.init = &init; 130 err = clk_hw_register(NULL, &ilp->hw); 131 if (err) 132 goto err_free_ilp; 133 134 err = of_clk_add_hw_provider(np, of_clk_hw_simple_get, &ilp->hw); 135 if (err) 136 goto err_clk_hw_unregister; 137 138 return; 139 140 err_clk_hw_unregister: 141 clk_hw_unregister(&ilp->hw); 142 err_free_ilp: 143 kfree(ilp); 144 pr_err("Failed to init ILP clock: %d\n", err); 145 } 146 147 /* We need it very early for arch code, before device model gets ready */ 148 CLK_OF_DECLARE(bcm53573_ilp_clk, "brcm,bcm53573-ilp", bcm53573_ilp_init); 149