123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486 |
- #include <linux/clk.h>
- #include <linux/cpufreq.h>
- #include <linux/device.h>
- #include <linux/err.h>
- #include <linux/idr.h>
- #include <linux/mutex.h>
- #include <linux/pm_opp.h>
- #include <linux/slab.h>
- #include <linux/thermal.h>
- #include <linux/clock_cooling.h>
- struct clock_cooling_device {
- int id;
- struct device *dev;
- struct thermal_cooling_device *cdev;
- struct notifier_block clk_rate_change_nb;
- struct cpufreq_frequency_table *freq_table;
- unsigned long clock_state;
- unsigned long clock_val;
- struct clk *clk;
- struct mutex lock;
- };
- #define to_clock_cooling_device(x) \
- container_of(x, struct clock_cooling_device, clk_rate_change_nb)
- static DEFINE_IDR(clock_idr);
- static DEFINE_MUTEX(cooling_clock_lock);
- static int clock_cooling_get_idr(int *id)
- {
- int ret;
- mutex_lock(&cooling_clock_lock);
- ret = idr_alloc(&clock_idr, NULL, 0, 0, GFP_KERNEL);
- mutex_unlock(&cooling_clock_lock);
- if (unlikely(ret < 0))
- return ret;
- *id = ret;
- return 0;
- }
- static void release_idr(int id)
- {
- mutex_lock(&cooling_clock_lock);
- idr_remove(&clock_idr, id);
- mutex_unlock(&cooling_clock_lock);
- }
- enum clock_cooling_property {
- GET_LEVEL,
- GET_FREQ,
- GET_MAXL,
- };
- static int clock_cooling_get_property(struct clock_cooling_device *ccdev,
- unsigned long input,
- unsigned long *output,
- enum clock_cooling_property property)
- {
- int i;
- unsigned long max_level = 0, level = 0;
- unsigned int freq = CPUFREQ_ENTRY_INVALID;
- int descend = -1;
- struct cpufreq_frequency_table *pos, *table = ccdev->freq_table;
- if (!output)
- return -EINVAL;
- if (!table)
- return -EINVAL;
- cpufreq_for_each_valid_entry(pos, table) {
-
- if (freq == pos->frequency)
- continue;
-
- if (freq != CPUFREQ_ENTRY_INVALID && descend == -1)
- descend = freq > pos->frequency;
- freq = pos->frequency;
- max_level++;
- }
-
- if (max_level == 0)
- return -EINVAL;
-
- max_level--;
-
- if (property == GET_MAXL) {
- *output = max_level;
- return 0;
- }
- if (property == GET_FREQ)
- level = descend ? input : (max_level - input);
- i = 0;
- cpufreq_for_each_valid_entry(pos, table) {
-
- if (freq == pos->frequency)
- continue;
-
- freq = pos->frequency;
- if (property == GET_LEVEL && (unsigned int)input == freq) {
-
- *output = descend ? i : (max_level - i);
- return 0;
- }
- if (property == GET_FREQ && level == i) {
-
- *output = freq;
- return 0;
- }
- i++;
- }
- return -EINVAL;
- }
- unsigned long clock_cooling_get_level(struct thermal_cooling_device *cdev,
- unsigned long freq)
- {
- struct clock_cooling_device *ccdev = cdev->devdata;
- unsigned long val;
- if (clock_cooling_get_property(ccdev, (unsigned long)freq, &val,
- GET_LEVEL))
- return THERMAL_CSTATE_INVALID;
- return val;
- }
- EXPORT_SYMBOL_GPL(clock_cooling_get_level);
- static unsigned long
- clock_cooling_get_frequency(struct clock_cooling_device *ccdev,
- unsigned long level)
- {
- int ret = 0;
- unsigned long freq;
- ret = clock_cooling_get_property(ccdev, level, &freq, GET_FREQ);
- if (ret)
- return 0;
- return freq;
- }
- static int clock_cooling_apply(struct clock_cooling_device *ccdev,
- unsigned long cooling_state)
- {
- unsigned long clip_freq, cur_freq;
- int ret = 0;
-
-
- if (ccdev->clock_state == cooling_state)
- return 0;
- clip_freq = clock_cooling_get_frequency(ccdev, cooling_state);
- if (!clip_freq)
- return -EINVAL;
- cur_freq = clk_get_rate(ccdev->clk);
- mutex_lock(&ccdev->lock);
- ccdev->clock_state = cooling_state;
- ccdev->clock_val = clip_freq;
-
- if (cur_freq > clip_freq)
- ret = clk_set_rate(ccdev->clk, clip_freq);
- mutex_unlock(&ccdev->lock);
- return ret;
- }
- static int clock_cooling_clock_notifier(struct notifier_block *nb,
- unsigned long event, void *data)
- {
- struct clk_notifier_data *ndata = data;
- struct clock_cooling_device *ccdev = to_clock_cooling_device(nb);
- switch (event) {
- case PRE_RATE_CHANGE:
-
- if (ndata->new_rate > ccdev->clock_val)
- return NOTIFY_BAD;
-
- case POST_RATE_CHANGE:
- case ABORT_RATE_CHANGE:
- default:
- return NOTIFY_DONE;
- }
- }
- static int clock_cooling_get_max_state(struct thermal_cooling_device *cdev,
- unsigned long *state)
- {
- struct clock_cooling_device *ccdev = cdev->devdata;
- unsigned long count = 0;
- int ret;
- ret = clock_cooling_get_property(ccdev, 0, &count, GET_MAXL);
- if (!ret)
- *state = count;
- return ret;
- }
- static int clock_cooling_get_cur_state(struct thermal_cooling_device *cdev,
- unsigned long *state)
- {
- struct clock_cooling_device *ccdev = cdev->devdata;
- *state = ccdev->clock_state;
- return 0;
- }
- static int clock_cooling_set_cur_state(struct thermal_cooling_device *cdev,
- unsigned long state)
- {
- struct clock_cooling_device *clock_device = cdev->devdata;
- return clock_cooling_apply(clock_device, state);
- }
- static struct thermal_cooling_device_ops const clock_cooling_ops = {
- .get_max_state = clock_cooling_get_max_state,
- .get_cur_state = clock_cooling_get_cur_state,
- .set_cur_state = clock_cooling_set_cur_state,
- };
- struct thermal_cooling_device *
- clock_cooling_register(struct device *dev, const char *clock_name)
- {
- struct thermal_cooling_device *cdev;
- struct clock_cooling_device *ccdev = NULL;
- char dev_name[THERMAL_NAME_LENGTH];
- int ret = 0;
- ccdev = devm_kzalloc(dev, sizeof(*ccdev), GFP_KERNEL);
- if (!ccdev)
- return ERR_PTR(-ENOMEM);
- mutex_init(&ccdev->lock);
- ccdev->dev = dev;
- ccdev->clk = devm_clk_get(dev, clock_name);
- if (IS_ERR(ccdev->clk))
- return ERR_CAST(ccdev->clk);
- ret = clock_cooling_get_idr(&ccdev->id);
- if (ret)
- return ERR_PTR(-EINVAL);
- snprintf(dev_name, sizeof(dev_name), "thermal-clock-%d", ccdev->id);
- cdev = thermal_cooling_device_register(dev_name, ccdev,
- &clock_cooling_ops);
- if (IS_ERR(cdev)) {
- release_idr(ccdev->id);
- return ERR_PTR(-EINVAL);
- }
- ccdev->cdev = cdev;
- ccdev->clk_rate_change_nb.notifier_call = clock_cooling_clock_notifier;
-
- ret = dev_pm_opp_init_cpufreq_table(dev, &ccdev->freq_table);
- if (ret) {
- release_idr(ccdev->id);
- return ERR_PTR(ret);
- }
- ccdev->clock_state = 0;
- ccdev->clock_val = clock_cooling_get_frequency(ccdev, 0);
- clk_notifier_register(ccdev->clk, &ccdev->clk_rate_change_nb);
- return cdev;
- }
- EXPORT_SYMBOL_GPL(clock_cooling_register);
- void clock_cooling_unregister(struct thermal_cooling_device *cdev)
- {
- struct clock_cooling_device *ccdev;
- if (!cdev)
- return;
- ccdev = cdev->devdata;
- clk_notifier_unregister(ccdev->clk, &ccdev->clk_rate_change_nb);
- dev_pm_opp_free_cpufreq_table(ccdev->dev, &ccdev->freq_table);
- thermal_cooling_device_unregister(ccdev->cdev);
- release_idr(ccdev->id);
- }
- EXPORT_SYMBOL_GPL(clock_cooling_unregister);
|