drivers/cpuidle/cpuidle-psci-domain.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * PM domains for CPUs via genpd - managed by cpuidle-psci.
   4  *
   5  * Copyright (C) 2019 Linaro Ltd.
   6  * Author: Ulf Hansson <ulf.hansson@linaro.org>
   7  *
   8  */
   9
  10 #define pr_fmt(fmt) "CPUidle PSCI: " fmt
  11
  12 #include <linux/cpu.h>
  13 #include <linux/device.h>
  14 #include <linux/kernel.h>
  15 #include <linux/pm_domain.h>
  16 #include <linux/pm_runtime.h>
  17 #include <linux/psci.h>
  18 #include <linux/slab.h>
  19 #include <linux/string.h>
  20
  21 #include "cpuidle-psci.h"
  22
  23 struct psci_pd_provider {
  24         struct list_head link;
  25         struct device_node *node;
  26 };
  27
  28 static LIST_HEAD(psci_pd_providers);
  29 static bool osi_mode_enabled __initdata;
  30
  31 static int psci_pd_power_off(struct generic_pm_domain *pd)
  32 {
  33         struct genpd_power_state *state = &pd->states[pd->state_idx];
  34         u32 *pd_state;
  35
  36         if (!state->data)
  37                 return 0;
  38
  39         /* OSI mode is enabled, set the corresponding domain state. */
  40         pd_state = state->data;
  41         psci_set_domain_state(*pd_state);
  42
  43         return 0;
  44 }
  45
  46 static int __init psci_pd_parse_state_nodes(struct genpd_power_state *states,
  47                                         int state_count)
  48 {
  49         int i, ret;
  50         u32 psci_state, *psci_state_buf;
  51
  52         for (i = 0; i < state_count; i++) {
  53                 ret = psci_dt_parse_state_node(to_of_node(states[i].fwnode),
  54                                         &psci_state);
  55                 if (ret)
  56                         goto free_state;
  57
  58                 psci_state_buf = kmalloc(sizeof(u32), GFP_KERNEL);
  59                 if (!psci_state_buf) {
  60                         ret = -ENOMEM;
  61                         goto free_state;
  62                 }
  63                 *psci_state_buf = psci_state;
  64                 states[i].data = psci_state_buf;
  65         }
  66
  67         return 0;
  68
  69 free_state:
  70         i--;
  71         for (; i >= 0; i--)
  72                 kfree(states[i].data);
  73         return ret;
  74 }
  75
  76 static int __init psci_pd_parse_states(struct device_node *np,
  77                         struct genpd_power_state **states, int *state_count)
  78 {
  79         int ret;
  80
  81         /* Parse the domain idle states. */
  82         ret = of_genpd_parse_idle_states(np, states, state_count);
  83         if (ret)
  84                 return ret;
  85
  86         /* Fill out the PSCI specifics for each found state. */
  87         ret = psci_pd_parse_state_nodes(*states, *state_count);
  88         if (ret)
  89                 kfree(*states);
  90
  91         return ret;
  92 }
  93
  94 static void psci_pd_free_states(struct genpd_power_state *states,
  95                                 unsigned int state_count)
  96 {
  97         int i;
  98
  99         for (i = 0; i < state_count; i++)
 100                 kfree(states[i].data);
 101         kfree(states);
 102 }
 103
 104 static int __init psci_pd_init(struct device_node *np)
 105 {
 106         struct generic_pm_domain *pd;
 107         struct psci_pd_provider *pd_provider;
 108         struct dev_power_governor *pd_gov;
 109         struct genpd_power_state *states = NULL;
 110         int ret = -ENOMEM, state_count = 0;
 111
 112         pd = kzalloc(sizeof(*pd), GFP_KERNEL);
 113         if (!pd)
 114                 goto out;
 115
 116         pd_provider = kzalloc(sizeof(*pd_provider), GFP_KERNEL);
 117         if (!pd_provider)
 118                 goto free_pd;
 119
 120         pd->name = kasprintf(GFP_KERNEL, "%pOF", np);
 121         if (!pd->name)
 122                 goto free_pd_prov;
 123
 124         /*
 125          * Parse the domain idle states and let genpd manage the state selection
 126          * for those being compatible with "domain-idle-state".
 127          */
 128         ret = psci_pd_parse_states(np, &states, &state_count);
 129         if (ret)
 130                 goto free_name;
 131
 132         pd->free_states = psci_pd_free_states;
 133         pd->name = kbasename(pd->name);
 134         pd->power_off = psci_pd_power_off;
 135         pd->states = states;
 136         pd->state_count = state_count;
 137         pd->flags |= GENPD_FLAG_IRQ_SAFE | GENPD_FLAG_CPU_DOMAIN;
 138
 139         /* Use governor for CPU PM domains if it has some states to manage. */
 140         pd_gov = state_count > 0 ? &pm_domain_cpu_gov : NULL;
 141
 142         ret = pm_genpd_init(pd, pd_gov, false);
 143         if (ret) {
 144                 psci_pd_free_states(states, state_count);
 145                 goto free_name;
 146         }
 147
 148         ret = of_genpd_add_provider_simple(np, pd);
 149         if (ret)
 150                 goto remove_pd;
 151
 152         pd_provider->node = of_node_get(np);
 153         list_add(&pd_provider->link, &psci_pd_providers);
 154
 155         pr_debug("init PM domain %s\n", pd->name);
 156         return 0;
 157
 158 remove_pd:
 159         pm_genpd_remove(pd);
 160 free_name:
 161         kfree(pd->name);
 162 free_pd_prov:
 163         kfree(pd_provider);
 164 free_pd:
 165         kfree(pd);
 166 out:
 167         pr_err("failed to init PM domain ret=%d %pOF\n", ret, np);
 168         return ret;
 169 }
 170
 171 static void __init psci_pd_remove(void)
 172 {
 173         struct psci_pd_provider *pd_provider, *it;
 174         struct generic_pm_domain *genpd;
 175
 176         list_for_each_entry_safe(pd_provider, it, &psci_pd_providers, link) {
 177                 of_genpd_del_provider(pd_provider->node);
 178
 179                 genpd = of_genpd_remove_last(pd_provider->node);
 180                 if (!IS_ERR(genpd))
 181                         kfree(genpd);
 182
 183                 of_node_put(pd_provider->node);
 184                 list_del(&pd_provider->link);
 185                 kfree(pd_provider);
 186         }
 187 }
 188
 189 static int __init psci_pd_init_topology(struct device_node *np, bool add)
 190 {
 191         struct device_node *node;
 192         struct of_phandle_args child, parent;
 193         int ret;
 194
 195         for_each_child_of_node(np, node) {
 196                 if (of_parse_phandle_with_args(node, "power-domains",
 197                                         "#power-domain-cells", 0, &parent))
 198                         continue;
 199
 200                 child.np = node;
 201                 child.args_count = 0;
 202
 203                 ret = add ? of_genpd_add_subdomain(&parent, &child) :
 204                         of_genpd_remove_subdomain(&parent, &child);
 205                 of_node_put(parent.np);
 206                 if (ret) {
 207                         of_node_put(node);
 208                         return ret;
 209                 }
 210         }
 211
 212         return 0;
 213 }
 214
 215 static int __init psci_pd_add_topology(struct device_node *np)
 216 {
 217         return psci_pd_init_topology(np, true);
 218 }
 219
 220 static void __init psci_pd_remove_topology(struct device_node *np)
 221 {
 222         psci_pd_init_topology(np, false);
 223 }
 224
 225 static const struct of_device_id psci_of_match[] __initconst = {
 226         { .compatible = "arm,psci-1.0" },
 227         {}
 228 };
 229
 230 static int __init psci_idle_init_domains(void)
 231 {
 232         struct device_node *np = of_find_matching_node(NULL, psci_of_match);
 233         struct device_node *node;
 234         int ret = 0, pd_count = 0;
 235
 236         if (!np)
 237                 return -ENODEV;
 238
 239         /* Currently limit the hierarchical topology to be used in OSI mode. */
 240         if (!psci_has_osi_support())
 241                 goto out;
 242
 243         /*
 244          * Parse child nodes for the "#power-domain-cells" property and
 245          * initialize a genpd/genpd-of-provider pair when it's found.
 246          */
 247         for_each_child_of_node(np, node) {
 248                 if (!of_find_property(node, "#power-domain-cells", NULL))
 249                         continue;
 250
 251                 ret = psci_pd_init(node);
 252                 if (ret)
 253                         goto put_node;
 254
 255                 pd_count++;
 256         }
 257
 258         /* Bail out if not using the hierarchical CPU topology. */
 259         if (!pd_count)
 260                 goto out;
 261
 262         /* Link genpd masters/subdomains to model the CPU topology. */
 263         ret = psci_pd_add_topology(np);
 264         if (ret)
 265                 goto remove_pd;
 266
 267         /* Try to enable OSI mode. */
 268         ret = psci_set_osi_mode();
 269         if (ret) {
 270                 pr_warn("failed to enable OSI mode: %d\n", ret);
 271                 psci_pd_remove_topology(np);
 272                 goto remove_pd;
 273         }
 274
 275         osi_mode_enabled = true;
 276         of_node_put(np);
 277         pr_info("Initialized CPU PM domain topology\n");
 278         return pd_count;
 279
 280 put_node:
 281         of_node_put(node);
 282 remove_pd:
 283         if (pd_count)
 284                 psci_pd_remove();
 285         pr_err("failed to create CPU PM domains ret=%d\n", ret);
 286 out:
 287         of_node_put(np);
 288         return ret;
 289 }
 290 subsys_initcall(psci_idle_init_domains);
 291
 292 struct device __init *psci_dt_attach_cpu(int cpu)
 293 {
 294         struct device *dev;
 295
 296         if (!osi_mode_enabled)
 297                 return NULL;
 298
 299         dev = dev_pm_domain_attach_by_name(get_cpu_device(cpu), "psci");
 300         if (IS_ERR_OR_NULL(dev))
 301                 return dev;
 302
 303         pm_runtime_irq_safe(dev);
 304         if (cpu_online(cpu))
 305                 pm_runtime_get_sync(dev);
 306
 307         return dev;
 308 }