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