Saravana Kannan | 89445cf | 2013-11-22 16:46:16 -0800 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved. |
| 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 and |
| 6 | * only version 2 as published by the Free Software Foundation. |
| 7 | * |
| 8 | * This program is distributed in the hope that it will be useful, |
| 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 11 | * GNU General Public License for more details. |
| 12 | */ |
| 13 | |
| 14 | #define pr_fmt(fmt) "devbw: " fmt |
| 15 | |
| 16 | #include <linux/kernel.h> |
| 17 | #include <linux/module.h> |
| 18 | #include <linux/init.h> |
| 19 | #include <linux/io.h> |
| 20 | #include <linux/delay.h> |
| 21 | #include <linux/ktime.h> |
| 22 | #include <linux/time.h> |
| 23 | #include <linux/err.h> |
| 24 | #include <linux/errno.h> |
| 25 | #include <linux/mutex.h> |
| 26 | #include <linux/interrupt.h> |
| 27 | #include <linux/devfreq.h> |
| 28 | #include <linux/of.h> |
| 29 | #include <trace/events/power.h> |
| 30 | #include <linux/msm-bus.h> |
| 31 | #include <linux/msm-bus-board.h> |
| 32 | |
| 33 | /* Has to be ULL to prevent overflow where this macro is used. */ |
| 34 | #define MBYTE (1ULL << 20) |
| 35 | #define MAX_PATHS 2 |
| 36 | #define DBL_BUF 2 |
| 37 | |
| 38 | struct dev_data { |
| 39 | struct msm_bus_vectors vectors[MAX_PATHS * DBL_BUF]; |
| 40 | struct msm_bus_paths bw_levels[DBL_BUF]; |
| 41 | struct msm_bus_scale_pdata bw_data; |
| 42 | int num_paths; |
| 43 | u32 bus_client; |
| 44 | int cur_idx; |
| 45 | int cur_ab; |
| 46 | int cur_ib; |
| 47 | long gov_ab; |
| 48 | struct devfreq *df; |
| 49 | struct devfreq_dev_profile dp; |
| 50 | }; |
| 51 | |
| 52 | static int set_bw(struct device *dev, int new_ib, int new_ab) |
| 53 | { |
| 54 | struct dev_data *d = dev_get_drvdata(dev); |
| 55 | int i, ret; |
| 56 | |
| 57 | if (d->cur_ib == new_ib && d->cur_ab == new_ab) |
| 58 | return 0; |
| 59 | |
| 60 | i = (d->cur_idx + 1) % DBL_BUF; |
| 61 | |
| 62 | d->bw_levels[i].vectors[0].ib = new_ib * MBYTE; |
| 63 | d->bw_levels[i].vectors[0].ab = new_ab / d->num_paths * MBYTE; |
| 64 | d->bw_levels[i].vectors[1].ib = new_ib * MBYTE; |
| 65 | d->bw_levels[i].vectors[1].ab = new_ab / d->num_paths * MBYTE; |
| 66 | |
| 67 | dev_dbg(dev, "BW MBps: AB: %d IB: %d\n", new_ab, new_ib); |
| 68 | |
| 69 | ret = msm_bus_scale_client_update_request(d->bus_client, i); |
| 70 | if (ret) { |
| 71 | dev_err(dev, "bandwidth request failed (%d)\n", ret); |
| 72 | } else { |
| 73 | d->cur_idx = i; |
| 74 | d->cur_ib = new_ib; |
| 75 | d->cur_ab = new_ab; |
| 76 | } |
| 77 | |
| 78 | return ret; |
| 79 | } |
| 80 | |
| 81 | static void find_freq(struct devfreq_dev_profile *p, unsigned long *freq, |
| 82 | u32 flags) |
| 83 | { |
| 84 | int i; |
| 85 | unsigned long atmost, atleast, f; |
| 86 | |
| 87 | atmost = p->freq_table[0]; |
| 88 | atleast = p->freq_table[p->max_state-1]; |
| 89 | for (i = 0; i < p->max_state; i++) { |
| 90 | f = p->freq_table[i]; |
| 91 | if (f <= *freq) |
| 92 | atmost = max(f, atmost); |
| 93 | if (f >= *freq) |
| 94 | atleast = min(f, atleast); |
| 95 | } |
| 96 | |
| 97 | if (flags & DEVFREQ_FLAG_LEAST_UPPER_BOUND) |
| 98 | *freq = atmost; |
| 99 | else |
| 100 | *freq = atleast; |
| 101 | } |
| 102 | |
| 103 | static int devbw_target(struct device *dev, unsigned long *freq, u32 flags) |
| 104 | { |
| 105 | struct dev_data *d = dev_get_drvdata(dev); |
| 106 | |
| 107 | find_freq(&d->dp, freq, flags); |
| 108 | return set_bw(dev, *freq, d->gov_ab); |
| 109 | } |
| 110 | |
| 111 | static int devbw_get_dev_status(struct device *dev, |
| 112 | struct devfreq_dev_status *stat) |
| 113 | { |
| 114 | struct dev_data *d = dev_get_drvdata(dev); |
| 115 | |
| 116 | stat->private_data = &d->gov_ab; |
| 117 | return 0; |
| 118 | } |
| 119 | |
| 120 | #define PROP_PORTS "qcom,src-dst-ports" |
| 121 | #define PROP_TBL "qcom,bw-tbl" |
| 122 | #define PROP_ACTIVE "qcom,active-only" |
| 123 | |
| 124 | int devfreq_add_devbw(struct device *dev) |
| 125 | { |
| 126 | struct dev_data *d; |
| 127 | struct devfreq_dev_profile *p; |
| 128 | u32 *data, ports[MAX_PATHS * 2]; |
| 129 | const char *gov_name; |
| 130 | int ret, len, i, num_paths; |
| 131 | |
| 132 | d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL); |
| 133 | if (!d) |
| 134 | return -ENOMEM; |
| 135 | dev_set_drvdata(dev, d); |
| 136 | |
| 137 | if (of_find_property(dev->of_node, PROP_PORTS, &len)) { |
| 138 | len /= sizeof(ports[0]); |
| 139 | if (len % 2 || len > ARRAY_SIZE(ports)) { |
| 140 | dev_err(dev, "Unexpected number of ports\n"); |
| 141 | return -EINVAL; |
| 142 | } |
| 143 | |
| 144 | ret = of_property_read_u32_array(dev->of_node, PROP_PORTS, |
| 145 | ports, len); |
| 146 | if (ret) |
| 147 | return ret; |
| 148 | |
| 149 | num_paths = len / 2; |
| 150 | } else { |
| 151 | return -EINVAL; |
| 152 | } |
| 153 | |
| 154 | d->bw_levels[0].vectors = &d->vectors[0]; |
| 155 | d->bw_levels[1].vectors = &d->vectors[MAX_PATHS]; |
| 156 | d->bw_data.usecase = d->bw_levels; |
| 157 | d->bw_data.num_usecases = ARRAY_SIZE(d->bw_levels); |
| 158 | d->bw_data.name = dev_name(dev); |
| 159 | d->bw_data.active_only = of_property_read_bool(dev->of_node, |
| 160 | PROP_ACTIVE); |
| 161 | |
| 162 | for (i = 0; i < num_paths; i++) { |
| 163 | d->bw_levels[0].vectors[i].src = ports[2 * i]; |
| 164 | d->bw_levels[0].vectors[i].dst = ports[2 * i + 1]; |
| 165 | d->bw_levels[1].vectors[i].src = ports[2 * i]; |
| 166 | d->bw_levels[1].vectors[i].dst = ports[2 * i + 1]; |
| 167 | } |
| 168 | d->bw_levels[0].num_paths = num_paths; |
| 169 | d->bw_levels[1].num_paths = num_paths; |
| 170 | d->num_paths = num_paths; |
| 171 | |
| 172 | p = &d->dp; |
| 173 | p->polling_ms = 50; |
| 174 | p->target = devbw_target; |
| 175 | p->get_dev_status = devbw_get_dev_status; |
| 176 | |
| 177 | if (of_find_property(dev->of_node, PROP_TBL, &len)) { |
| 178 | len /= sizeof(*data); |
| 179 | data = devm_kzalloc(dev, len * sizeof(*data), GFP_KERNEL); |
| 180 | if (!data) |
| 181 | return -ENOMEM; |
| 182 | |
| 183 | p->freq_table = devm_kzalloc(dev, |
| 184 | len * sizeof(*p->freq_table), |
| 185 | GFP_KERNEL); |
| 186 | if (!p->freq_table) |
| 187 | return -ENOMEM; |
| 188 | |
| 189 | ret = of_property_read_u32_array(dev->of_node, PROP_TBL, |
| 190 | data, len); |
| 191 | if (ret) |
| 192 | return ret; |
| 193 | |
| 194 | for (i = 0; i < len; i++) |
| 195 | p->freq_table[i] = data[i]; |
| 196 | p->max_state = len; |
| 197 | } |
| 198 | |
| 199 | d->bus_client = msm_bus_scale_register_client(&d->bw_data); |
| 200 | if (!d->bus_client) { |
| 201 | dev_err(dev, "Unable to register bus client\n"); |
| 202 | return -ENODEV; |
| 203 | } |
| 204 | |
| 205 | if (of_property_read_string(dev->of_node, "governor", &gov_name)) |
| 206 | gov_name = "performance"; |
| 207 | |
| 208 | d->df = devfreq_add_device(dev, p, gov_name, NULL); |
| 209 | if (IS_ERR(d->df)) { |
| 210 | msm_bus_scale_unregister_client(d->bus_client); |
| 211 | return PTR_ERR(d->df); |
| 212 | } |
| 213 | |
| 214 | return 0; |
| 215 | } |
| 216 | |
Saravana Kannan | 89445cf | 2013-11-22 16:46:16 -0800 | [diff] [blame] | 217 | int devfreq_remove_devbw(struct device *dev) |
| 218 | { |
| 219 | struct dev_data *d = dev_get_drvdata(dev); |
| 220 | |
| 221 | msm_bus_scale_unregister_client(d->bus_client); |
| 222 | devfreq_remove_device(d->df); |
| 223 | return 0; |
| 224 | } |
| 225 | |
Saravana Kannan | 217b9b1 | 2014-08-07 20:04:45 -0700 | [diff] [blame] | 226 | int devfreq_suspend_devbw(struct device *dev) |
| 227 | { |
| 228 | struct dev_data *d = dev_get_drvdata(dev); |
| 229 | |
| 230 | return devfreq_suspend_device(d->df); |
| 231 | } |
| 232 | |
| 233 | int devfreq_resume_devbw(struct device *dev) |
| 234 | { |
| 235 | struct dev_data *d = dev_get_drvdata(dev); |
| 236 | |
| 237 | return devfreq_resume_device(d->df); |
| 238 | } |
| 239 | |
| 240 | static int devfreq_devbw_probe(struct platform_device *pdev) |
| 241 | { |
| 242 | return devfreq_add_devbw(&pdev->dev); |
| 243 | } |
| 244 | |
Saravana Kannan | 89445cf | 2013-11-22 16:46:16 -0800 | [diff] [blame] | 245 | static int devfreq_devbw_remove(struct platform_device *pdev) |
| 246 | { |
| 247 | return devfreq_remove_devbw(&pdev->dev); |
| 248 | } |
| 249 | |
| 250 | static const struct of_device_id devbw_match_table[] = { |
| 251 | { .compatible = "qcom,devbw" }, |
| 252 | {} |
| 253 | }; |
| 254 | |
| 255 | static struct platform_driver devbw_driver = { |
| 256 | .probe = devfreq_devbw_probe, |
| 257 | .remove = devfreq_devbw_remove, |
| 258 | .driver = { |
| 259 | .name = "devbw", |
| 260 | .of_match_table = devbw_match_table, |
| 261 | }, |
| 262 | }; |
| 263 | |
| 264 | module_platform_driver(devbw_driver); |
| 265 | MODULE_DESCRIPTION("Device DDR bandwidth voting driver MSM SoCs"); |
| 266 | MODULE_LICENSE("GPL v2"); |