Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 1 | /* |
Santosh Mardi | e4d5b2d | 2018-06-27 15:26:06 +0530 | [diff] [blame] | 2 | * Copyright (c) 2014-2015, 2017-2018, The Linux Foundation. |
| 3 | * All rights reserved. |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 4 | * |
| 5 | * This program is free software; you can redistribute it and/or modify |
| 6 | * it under the terms of the GNU General Public License version 2 and |
| 7 | * only version 2 as published by the Free Software Foundation. |
| 8 | * |
| 9 | * This program is distributed in the hope that it will be useful, |
| 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 12 | * GNU General Public License for more details. |
| 13 | */ |
| 14 | |
| 15 | #define pr_fmt(fmt) "devfreq-simple-dev: " fmt |
| 16 | |
| 17 | #include <linux/kernel.h> |
| 18 | #include <linux/module.h> |
| 19 | #include <linux/init.h> |
| 20 | #include <linux/io.h> |
| 21 | #include <linux/delay.h> |
| 22 | #include <linux/ktime.h> |
| 23 | #include <linux/time.h> |
| 24 | #include <linux/err.h> |
| 25 | #include <linux/errno.h> |
| 26 | #include <linux/mutex.h> |
| 27 | #include <linux/platform_device.h> |
| 28 | #include <linux/interrupt.h> |
| 29 | #include <linux/devfreq.h> |
| 30 | #include <linux/of.h> |
| 31 | #include <linux/clk.h> |
| 32 | #include <trace/events/power.h> |
| 33 | |
| 34 | struct dev_data { |
| 35 | struct clk *clk; |
| 36 | struct devfreq *df; |
| 37 | struct devfreq_dev_profile profile; |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 38 | bool freq_in_khz; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 39 | }; |
| 40 | |
| 41 | static void find_freq(struct devfreq_dev_profile *p, unsigned long *freq, |
| 42 | u32 flags) |
| 43 | { |
| 44 | int i; |
| 45 | unsigned long atmost, atleast, f; |
| 46 | |
| 47 | atmost = p->freq_table[0]; |
| 48 | atleast = p->freq_table[p->max_state-1]; |
| 49 | for (i = 0; i < p->max_state; i++) { |
| 50 | f = p->freq_table[i]; |
| 51 | if (f <= *freq) |
| 52 | atmost = max(f, atmost); |
| 53 | if (f >= *freq) |
| 54 | atleast = min(f, atleast); |
| 55 | } |
| 56 | |
| 57 | if (flags & DEVFREQ_FLAG_LEAST_UPPER_BOUND) |
| 58 | *freq = atmost; |
| 59 | else |
| 60 | *freq = atleast; |
| 61 | } |
| 62 | |
| 63 | static int dev_target(struct device *dev, unsigned long *freq, u32 flags) |
| 64 | { |
| 65 | struct dev_data *d = dev_get_drvdata(dev); |
| 66 | unsigned long rfreq; |
| 67 | |
| 68 | find_freq(&d->profile, freq, flags); |
| 69 | |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 70 | rfreq = clk_round_rate(d->clk, d->freq_in_khz ? *freq * 1000 : *freq); |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 71 | if (IS_ERR_VALUE(rfreq)) { |
| 72 | dev_err(dev, "devfreq: Cannot find matching frequency for %lu\n", |
| 73 | *freq); |
| 74 | return rfreq; |
| 75 | } |
| 76 | |
| 77 | return clk_set_rate(d->clk, rfreq); |
| 78 | } |
| 79 | |
| 80 | static int dev_get_cur_freq(struct device *dev, unsigned long *freq) |
| 81 | { |
| 82 | struct dev_data *d = dev_get_drvdata(dev); |
| 83 | unsigned long f; |
| 84 | |
| 85 | f = clk_get_rate(d->clk); |
| 86 | if (IS_ERR_VALUE(f)) |
| 87 | return f; |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 88 | *freq = d->freq_in_khz ? f / 1000 : f; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 89 | return 0; |
| 90 | } |
| 91 | |
| 92 | #define PROP_TBL "freq-tbl-khz" |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 93 | static int parse_freq_table(struct device *dev, struct dev_data *d) |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 94 | { |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 95 | struct devfreq_dev_profile *p = &d->profile; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 96 | int ret, len, i, j; |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 97 | u32 *data; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 98 | unsigned long f; |
| 99 | |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 100 | if (!of_find_property(dev->of_node, PROP_TBL, &len)) { |
| 101 | if (dev_pm_opp_get_opp_count(dev) <= 0) |
| 102 | return -EPROBE_DEFER; |
| 103 | return 0; |
| 104 | } |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 105 | |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 106 | d->freq_in_khz = true; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 107 | len /= sizeof(*data); |
| 108 | data = devm_kzalloc(dev, len * sizeof(*data), GFP_KERNEL); |
| 109 | if (!data) |
| 110 | return -ENOMEM; |
| 111 | |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 112 | p->freq_table = devm_kzalloc(dev, len * sizeof(*p->freq_table), |
| 113 | GFP_KERNEL); |
| 114 | if (!p->freq_table) |
| 115 | return -ENOMEM; |
| 116 | |
| 117 | ret = of_property_read_u32_array(dev->of_node, PROP_TBL, data, len); |
| 118 | if (ret) |
| 119 | return ret; |
| 120 | |
| 121 | j = 0; |
| 122 | for (i = 0; i < len; i++) { |
| 123 | f = clk_round_rate(d->clk, data[i] * 1000); |
| 124 | if (IS_ERR_VALUE(f)) |
| 125 | dev_warn(dev, "Unable to find dev rate for %d KHz", |
| 126 | data[i]); |
| 127 | else |
| 128 | p->freq_table[j++] = f / 1000; |
| 129 | } |
| 130 | p->max_state = j; |
| 131 | devm_kfree(dev, data); |
| 132 | |
| 133 | if (p->max_state == 0) { |
| 134 | dev_err(dev, "Error parsing property %s!\n", PROP_TBL); |
| 135 | return -EINVAL; |
| 136 | } |
| 137 | |
Rohit Gupta | aa422df | 2017-05-25 13:02:46 -0700 | [diff] [blame] | 138 | return 0; |
| 139 | } |
| 140 | |
| 141 | static int devfreq_clock_probe(struct platform_device *pdev) |
| 142 | { |
| 143 | struct device *dev = &pdev->dev; |
| 144 | struct dev_data *d; |
| 145 | struct devfreq_dev_profile *p; |
| 146 | u32 poll; |
| 147 | const char *gov_name; |
| 148 | int ret; |
| 149 | |
| 150 | d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL); |
| 151 | if (!d) |
| 152 | return -ENOMEM; |
| 153 | platform_set_drvdata(pdev, d); |
| 154 | |
| 155 | d->clk = devm_clk_get(dev, "devfreq_clk"); |
| 156 | if (IS_ERR(d->clk)) |
| 157 | return PTR_ERR(d->clk); |
| 158 | |
| 159 | ret = parse_freq_table(dev, d); |
| 160 | if (ret) |
| 161 | return ret; |
| 162 | |
| 163 | p = &d->profile; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 164 | p->target = dev_target; |
| 165 | p->get_cur_freq = dev_get_cur_freq; |
| 166 | ret = dev_get_cur_freq(dev, &p->initial_freq); |
| 167 | if (ret) |
| 168 | return ret; |
| 169 | |
| 170 | p->polling_ms = 50; |
| 171 | if (!of_property_read_u32(dev->of_node, "polling-ms", &poll)) |
| 172 | p->polling_ms = poll; |
| 173 | |
| 174 | if (of_property_read_string(dev->of_node, "governor", &gov_name)) |
| 175 | gov_name = "performance"; |
| 176 | |
Junjie Wu | 9864733 | 2015-03-03 17:15:35 -0800 | [diff] [blame] | 177 | if (of_property_read_bool(dev->of_node, "qcom,prepare-clk")) { |
| 178 | ret = clk_prepare(d->clk); |
| 179 | if (ret) |
| 180 | return ret; |
| 181 | } |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 182 | |
| 183 | d->df = devfreq_add_device(dev, p, gov_name, NULL); |
Junjie Wu | 9864733 | 2015-03-03 17:15:35 -0800 | [diff] [blame] | 184 | if (IS_ERR(d->df)) { |
| 185 | ret = PTR_ERR(d->df); |
| 186 | goto add_err; |
| 187 | } |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 188 | |
| 189 | return 0; |
Junjie Wu | 9864733 | 2015-03-03 17:15:35 -0800 | [diff] [blame] | 190 | add_err: |
| 191 | if (of_property_read_bool(dev->of_node, "qcom,prepare-clk")) |
| 192 | clk_unprepare(d->clk); |
| 193 | return ret; |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 194 | } |
| 195 | |
| 196 | static int devfreq_clock_remove(struct platform_device *pdev) |
| 197 | { |
| 198 | struct dev_data *d = platform_get_drvdata(pdev); |
| 199 | |
| 200 | devfreq_remove_device(d->df); |
| 201 | |
| 202 | return 0; |
| 203 | } |
| 204 | |
| 205 | static const struct of_device_id devfreq_simple_match_table[] = { |
| 206 | { .compatible = "devfreq-simple-dev" }, |
| 207 | {} |
| 208 | }; |
| 209 | |
| 210 | static struct platform_driver devfreq_clock_driver = { |
| 211 | .probe = devfreq_clock_probe, |
| 212 | .remove = devfreq_clock_remove, |
| 213 | .driver = { |
| 214 | .name = "devfreq-simple-dev", |
| 215 | .of_match_table = devfreq_simple_match_table, |
Santosh Mardi | e4d5b2d | 2018-06-27 15:26:06 +0530 | [diff] [blame] | 216 | .suppress_bind_attrs = true, |
Saravana Kannan | d905e65 | 2014-01-21 18:49:37 -0800 | [diff] [blame] | 217 | }, |
| 218 | }; |
| 219 | module_platform_driver(devfreq_clock_driver); |
| 220 | MODULE_DESCRIPTION("Devfreq driver for setting generic device clock frequency"); |
| 221 | MODULE_LICENSE("GPL v2"); |