362 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * Copyright (c) 2016-2017, NVIDIA CORPORATION. All rights reserved4 */5 6#include <linux/of.h>7#include <linux/platform_device.h>8#include <linux/pm_domain.h>9#include <linux/slab.h>10 11#include <soc/tegra/bpmp.h>12#include <soc/tegra/bpmp-abi.h>13 14struct tegra_powergate_info {15 unsigned int id;16 char *name;17};18 19struct tegra_powergate {20 struct generic_pm_domain genpd;21 struct tegra_bpmp *bpmp;22 unsigned int id;23};24 25static inline struct tegra_powergate *26to_tegra_powergate(struct generic_pm_domain *genpd)27{28 return container_of(genpd, struct tegra_powergate, genpd);29}30 31static int tegra_bpmp_powergate_set_state(struct tegra_bpmp *bpmp,32 unsigned int id, u32 state)33{34 struct mrq_pg_request request;35 struct tegra_bpmp_message msg;36 int err;37 38 memset(&request, 0, sizeof(request));39 request.cmd = CMD_PG_SET_STATE;40 request.id = id;41 request.set_state.state = state;42 43 memset(&msg, 0, sizeof(msg));44 msg.mrq = MRQ_PG;45 msg.tx.data = &request;46 msg.tx.size = sizeof(request);47 48 err = tegra_bpmp_transfer(bpmp, &msg);49 if (err < 0)50 return err;51 else if (msg.rx.ret < 0)52 return -EINVAL;53 54 return 0;55}56 57static int tegra_bpmp_powergate_get_state(struct tegra_bpmp *bpmp,58 unsigned int id)59{60 struct mrq_pg_response response;61 struct mrq_pg_request request;62 struct tegra_bpmp_message msg;63 int err;64 65 memset(&request, 0, sizeof(request));66 request.cmd = CMD_PG_GET_STATE;67 request.id = id;68 69 memset(&response, 0, sizeof(response));70 71 memset(&msg, 0, sizeof(msg));72 msg.mrq = MRQ_PG;73 msg.tx.data = &request;74 msg.tx.size = sizeof(request);75 msg.rx.data = &response;76 msg.rx.size = sizeof(response);77 78 err = tegra_bpmp_transfer(bpmp, &msg);79 if (err < 0)80 return PG_STATE_OFF;81 else if (msg.rx.ret < 0)82 return -EINVAL;83 84 return response.get_state.state;85}86 87static int tegra_bpmp_powergate_get_max_id(struct tegra_bpmp *bpmp)88{89 struct mrq_pg_response response;90 struct mrq_pg_request request;91 struct tegra_bpmp_message msg;92 int err;93 94 memset(&request, 0, sizeof(request));95 request.cmd = CMD_PG_GET_MAX_ID;96 97 memset(&response, 0, sizeof(response));98 99 memset(&msg, 0, sizeof(msg));100 msg.mrq = MRQ_PG;101 msg.tx.data = &request;102 msg.tx.size = sizeof(request);103 msg.rx.data = &response;104 msg.rx.size = sizeof(response);105 106 err = tegra_bpmp_transfer(bpmp, &msg);107 if (err < 0)108 return err;109 else if (msg.rx.ret < 0)110 return -EINVAL;111 112 return response.get_max_id.max_id;113}114 115static char *tegra_bpmp_powergate_get_name(struct tegra_bpmp *bpmp,116 unsigned int id)117{118 struct mrq_pg_response response;119 struct mrq_pg_request request;120 struct tegra_bpmp_message msg;121 int err;122 123 memset(&request, 0, sizeof(request));124 request.cmd = CMD_PG_GET_NAME;125 request.id = id;126 127 memset(&response, 0, sizeof(response));128 129 memset(&msg, 0, sizeof(msg));130 msg.mrq = MRQ_PG;131 msg.tx.data = &request;132 msg.tx.size = sizeof(request);133 msg.rx.data = &response;134 msg.rx.size = sizeof(response);135 136 err = tegra_bpmp_transfer(bpmp, &msg);137 if (err < 0 || msg.rx.ret < 0)138 return NULL;139 140 return kstrdup(response.get_name.name, GFP_KERNEL);141}142 143static inline bool tegra_bpmp_powergate_is_powered(struct tegra_bpmp *bpmp,144 unsigned int id)145{146 return tegra_bpmp_powergate_get_state(bpmp, id) != PG_STATE_OFF;147}148 149static int tegra_powergate_power_on(struct generic_pm_domain *domain)150{151 struct tegra_powergate *powergate = to_tegra_powergate(domain);152 struct tegra_bpmp *bpmp = powergate->bpmp;153 154 return tegra_bpmp_powergate_set_state(bpmp, powergate->id,155 PG_STATE_ON);156}157 158static int tegra_powergate_power_off(struct generic_pm_domain *domain)159{160 struct tegra_powergate *powergate = to_tegra_powergate(domain);161 struct tegra_bpmp *bpmp = powergate->bpmp;162 163 return tegra_bpmp_powergate_set_state(bpmp, powergate->id,164 PG_STATE_OFF);165}166 167static struct tegra_powergate *168tegra_powergate_add(struct tegra_bpmp *bpmp,169 const struct tegra_powergate_info *info)170{171 struct tegra_powergate *powergate;172 bool off;173 int err;174 175 off = !tegra_bpmp_powergate_is_powered(bpmp, info->id);176 177 powergate = devm_kzalloc(bpmp->dev, sizeof(*powergate), GFP_KERNEL);178 if (!powergate)179 return ERR_PTR(-ENOMEM);180 181 powergate->id = info->id;182 powergate->bpmp = bpmp;183 184 powergate->genpd.name = kstrdup(info->name, GFP_KERNEL);185 powergate->genpd.power_on = tegra_powergate_power_on;186 powergate->genpd.power_off = tegra_powergate_power_off;187 188 err = pm_genpd_init(&powergate->genpd, NULL, off);189 if (err < 0) {190 kfree(powergate->genpd.name);191 return ERR_PTR(err);192 }193 194 return powergate;195}196 197static void tegra_powergate_remove(struct tegra_powergate *powergate)198{199 struct generic_pm_domain *genpd = &powergate->genpd;200 struct tegra_bpmp *bpmp = powergate->bpmp;201 int err;202 203 err = pm_genpd_remove(genpd);204 if (err < 0)205 dev_err(bpmp->dev, "failed to remove power domain %s: %d\n",206 genpd->name, err);207 208 kfree(genpd->name);209}210 211static int212tegra_bpmp_probe_powergates(struct tegra_bpmp *bpmp,213 struct tegra_powergate_info **powergatesp)214{215 struct tegra_powergate_info *powergates;216 unsigned int max_id, id, count = 0;217 unsigned int num_holes = 0;218 int err;219 220 err = tegra_bpmp_powergate_get_max_id(bpmp);221 if (err < 0)222 return err;223 224 max_id = err;225 226 dev_dbg(bpmp->dev, "maximum powergate ID: %u\n", max_id);227 228 powergates = kcalloc(max_id + 1, sizeof(*powergates), GFP_KERNEL);229 if (!powergates)230 return -ENOMEM;231 232 for (id = 0; id <= max_id; id++) {233 struct tegra_powergate_info *info = &powergates[count];234 235 info->name = tegra_bpmp_powergate_get_name(bpmp, id);236 if (!info->name || info->name[0] == '\0') {237 num_holes++;238 continue;239 }240 241 info->id = id;242 count++;243 }244 245 dev_dbg(bpmp->dev, "holes: %u\n", num_holes);246 247 *powergatesp = powergates;248 249 return count;250}251 252static int tegra_bpmp_add_powergates(struct tegra_bpmp *bpmp,253 struct tegra_powergate_info *powergates,254 unsigned int count)255{256 struct genpd_onecell_data *genpd = &bpmp->genpd;257 struct generic_pm_domain **domains;258 struct tegra_powergate *powergate;259 unsigned int i;260 int err;261 262 domains = kcalloc(count, sizeof(*domains), GFP_KERNEL);263 if (!domains)264 return -ENOMEM;265 266 for (i = 0; i < count; i++) {267 powergate = tegra_powergate_add(bpmp, &powergates[i]);268 if (IS_ERR(powergate)) {269 err = PTR_ERR(powergate);270 goto remove;271 }272 273 dev_dbg(bpmp->dev, "added power domain %s\n",274 powergate->genpd.name);275 domains[i] = &powergate->genpd;276 }277 278 genpd->num_domains = count;279 genpd->domains = domains;280 281 return 0;282 283remove:284 while (i--) {285 powergate = to_tegra_powergate(domains[i]);286 tegra_powergate_remove(powergate);287 }288 289 kfree(domains);290 return err;291}292 293static void tegra_bpmp_remove_powergates(struct tegra_bpmp *bpmp)294{295 struct genpd_onecell_data *genpd = &bpmp->genpd;296 unsigned int i = genpd->num_domains;297 struct tegra_powergate *powergate;298 299 while (i--) {300 dev_dbg(bpmp->dev, "removing power domain %s\n",301 genpd->domains[i]->name);302 powergate = to_tegra_powergate(genpd->domains[i]);303 tegra_powergate_remove(powergate);304 }305}306 307static struct generic_pm_domain *308tegra_powergate_xlate(const struct of_phandle_args *spec, void *data)309{310 struct generic_pm_domain *domain = ERR_PTR(-ENOENT);311 struct genpd_onecell_data *genpd = data;312 unsigned int i;313 314 for (i = 0; i < genpd->num_domains; i++) {315 struct tegra_powergate *powergate;316 317 powergate = to_tegra_powergate(genpd->domains[i]);318 if (powergate->id == spec->args[0]) {319 domain = &powergate->genpd;320 break;321 }322 }323 324 return domain;325}326 327int tegra_bpmp_init_powergates(struct tegra_bpmp *bpmp)328{329 struct device_node *np = bpmp->dev->of_node;330 struct tegra_powergate_info *powergates;331 struct device *dev = bpmp->dev;332 unsigned int count, i;333 int err;334 335 err = tegra_bpmp_probe_powergates(bpmp, &powergates);336 if (err < 0)337 return err;338 339 count = err;340 341 dev_dbg(dev, "%u power domains probed\n", count);342 343 err = tegra_bpmp_add_powergates(bpmp, powergates, count);344 if (err < 0)345 goto free;346 347 bpmp->genpd.xlate = tegra_powergate_xlate;348 349 err = of_genpd_add_provider_onecell(np, &bpmp->genpd);350 if (err < 0) {351 dev_err(dev, "failed to add power domain provider: %d\n", err);352 tegra_bpmp_remove_powergates(bpmp);353 }354 355free:356 for (i = 0; i < count; i++)357 kfree(powergates[i].name);358 359 kfree(powergates);360 return err;361}362