diff --git a/drivers/clk/tegra/clk-bpmp.c b/drivers/clk/tegra/clk-bpmp.c new file mode 100644 index 00000000..3748a39d --- /dev/null +++ b/drivers/clk/tegra/clk-bpmp.c @@ -0,0 +1,673 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (C) 2016-2020 NVIDIA Corporation + */ + +#include +#include +#include +#include + +#include +#include + +#define TEGRA_BPMP_DUMP_CLOCK_INFO 0 + +#define TEGRA_BPMP_CLK_HAS_MUX BIT(0) +#define TEGRA_BPMP_CLK_HAS_SET_RATE BIT(1) +#define TEGRA_BPMP_CLK_IS_ROOT BIT(2) + +struct tegra_bpmp_clk_info { + unsigned int id; + char name[MRQ_CLK_NAME_MAXLEN]; + unsigned int parents[MRQ_CLK_MAX_PARENTS]; + unsigned int num_parents; + unsigned long flags; +}; + +struct tegra_bpmp_clk { + struct clk_hw hw; + + struct tegra_bpmp *bpmp; + unsigned int id; + + unsigned int num_parents; + unsigned int *parents; +}; + +static inline struct tegra_bpmp_clk *to_tegra_bpmp_clk(struct clk_hw *hw) +{ + return container_of(hw, struct tegra_bpmp_clk, hw); +} + +struct tegra_bpmp_clk_message { + unsigned int cmd; + unsigned int id; + + struct { + const void *data; + size_t size; + } tx; + + struct { + void *data; + size_t size; + int ret; + } rx; +}; + +static int tegra_bpmp_clk_transfer(struct tegra_bpmp *bpmp, + const struct tegra_bpmp_clk_message *clk) +{ + struct mrq_clk_request request; + struct tegra_bpmp_message msg; + void *req = &request; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd_and_id = (clk->cmd << 24) | clk->id; + + /* + * The mrq_clk_request structure has an anonymous union at offset 4 + * that contains all possible sub-command structures. Copy the data + * to that union. Ideally we'd be able to refer to it by name, but + * doing so would require changing the ABI header and increase the + * maintenance burden. + */ + memcpy(req + 4, clk->tx.data, clk->tx.size); + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_CLK; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = clk->rx.data; + msg.rx.size = clk->rx.size; + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + return 0; +} + +static int tegra_bpmp_clk_prepare(struct clk_hw *hw) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct tegra_bpmp_clk_message msg; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_ENABLE; + msg.id = clk->id; + + return tegra_bpmp_clk_transfer(clk->bpmp, &msg); +} + +static void tegra_bpmp_clk_unprepare(struct clk_hw *hw) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct tegra_bpmp_clk_message msg; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_DISABLE; + msg.id = clk->id; + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) + dev_err(clk->bpmp->dev, "failed to disable clock %s: %d\n", + clk_hw_get_name(hw), err); +} + +static int tegra_bpmp_clk_is_prepared(struct clk_hw *hw) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_is_enabled_response response; + struct tegra_bpmp_clk_message msg; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_IS_ENABLED; + msg.id = clk->id; + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) + return err; + + return response.state; +} + +static unsigned long tegra_bpmp_clk_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_get_rate_response response; + struct cmd_clk_get_rate_request request; + struct tegra_bpmp_clk_message msg; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_GET_RATE; + msg.id = clk->id; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) + return err; + + return response.rate; +} + +static int tegra_bpmp_clk_determine_rate(struct clk_hw *hw, + struct clk_rate_request *rate_req) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_round_rate_response response; + struct cmd_clk_round_rate_request request; + struct tegra_bpmp_clk_message msg; + unsigned long rate; + int err; + + rate = min(max(rate_req->rate, rate_req->min_rate), rate_req->max_rate); + + memset(&request, 0, sizeof(request)); + request.rate = min_t(u64, rate, S64_MAX); + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_ROUND_RATE; + msg.id = clk->id; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) + return err; + + rate_req->rate = (unsigned long)response.rate; + + return 0; +} + +static int tegra_bpmp_clk_set_parent(struct clk_hw *hw, u8 index) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_set_parent_response response; + struct cmd_clk_set_parent_request request; + struct tegra_bpmp_clk_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.parent_id = clk->parents[index]; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_SET_PARENT; + msg.id = clk->id; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) + return err; + + /* XXX check parent ID in response */ + + return 0; +} + +static u8 tegra_bpmp_clk_get_parent(struct clk_hw *hw) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_get_parent_response response; + struct tegra_bpmp_clk_message msg; + unsigned int i; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_GET_PARENT; + msg.id = clk->id; + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(clk->bpmp, &msg); + if (err < 0) { + dev_err(clk->bpmp->dev, "failed to get parent for %s: %d\n", + clk_hw_get_name(hw), err); + return U8_MAX; + } + + for (i = 0; i < clk->num_parents; i++) + if (clk->parents[i] == response.parent_id) + return i; + + return U8_MAX; +} + +static int tegra_bpmp_clk_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); + struct cmd_clk_set_rate_response response; + struct cmd_clk_set_rate_request request; + struct tegra_bpmp_clk_message msg; + + memset(&request, 0, sizeof(request)); + request.rate = min_t(u64, rate, S64_MAX); + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_SET_RATE; + msg.id = clk->id; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + return tegra_bpmp_clk_transfer(clk->bpmp, &msg); +} + +static const struct clk_ops tegra_bpmp_clk_gate_ops = { + .prepare = tegra_bpmp_clk_prepare, + .unprepare = tegra_bpmp_clk_unprepare, + .is_prepared = tegra_bpmp_clk_is_prepared, + .recalc_rate = tegra_bpmp_clk_recalc_rate, +}; + +static const struct clk_ops tegra_bpmp_clk_mux_ops = { + .prepare = tegra_bpmp_clk_prepare, + .unprepare = tegra_bpmp_clk_unprepare, + .is_prepared = tegra_bpmp_clk_is_prepared, + .recalc_rate = tegra_bpmp_clk_recalc_rate, + .set_parent = tegra_bpmp_clk_set_parent, + .get_parent = tegra_bpmp_clk_get_parent, +}; + +static const struct clk_ops tegra_bpmp_clk_rate_ops = { + .prepare = tegra_bpmp_clk_prepare, + .unprepare = tegra_bpmp_clk_unprepare, + .is_prepared = tegra_bpmp_clk_is_prepared, + .recalc_rate = tegra_bpmp_clk_recalc_rate, + .determine_rate = tegra_bpmp_clk_determine_rate, + .set_rate = tegra_bpmp_clk_set_rate, +}; + +static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = { + .prepare = tegra_bpmp_clk_prepare, + .unprepare = tegra_bpmp_clk_unprepare, + .is_prepared = tegra_bpmp_clk_is_prepared, + .recalc_rate = tegra_bpmp_clk_recalc_rate, + .determine_rate = tegra_bpmp_clk_determine_rate, + .set_parent = tegra_bpmp_clk_set_parent, + .get_parent = tegra_bpmp_clk_get_parent, + .set_rate = tegra_bpmp_clk_set_rate, +}; + +static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp) +{ + struct cmd_clk_get_max_clk_id_response response; + struct tegra_bpmp_clk_message msg; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_GET_MAX_CLK_ID; + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(bpmp, &msg); + if (err < 0) + return err; + + if (response.max_id > INT_MAX) + return -E2BIG; + + return response.max_id; +} + +static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id, + struct tegra_bpmp_clk_info *info) +{ + struct cmd_clk_get_all_info_response response; + struct tegra_bpmp_clk_message msg; + unsigned int i; + int err; + + memset(&msg, 0, sizeof(msg)); + msg.cmd = CMD_CLK_GET_ALL_INFO; + msg.id = id; + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_clk_transfer(bpmp, &msg); + if (err < 0) + return err; + + strlcpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN); + info->num_parents = response.num_parents; + + for (i = 0; i < info->num_parents; i++) + info->parents[i] = response.parents[i]; + + info->flags = response.flags; + + return 0; +} + +static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp, + const char *level, + const struct tegra_bpmp_clk_info *info) +{ + const char *prefix = ""; + struct seq_buf buf; + unsigned int i; + char flags[64]; + + seq_buf_init(&buf, flags, sizeof(flags)); + + if (info->flags) + seq_buf_printf(&buf, "("); + + if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) { + seq_buf_printf(&buf, "%smux", prefix); + prefix = ", "; + } + + if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) { + seq_buf_printf(&buf, "%sfixed", prefix); + prefix = ", "; + } + + if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) { + seq_buf_printf(&buf, "%sroot", prefix); + prefix = ", "; + } + + if (info->flags) + seq_buf_printf(&buf, ")"); + + dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name); + dev_printk(level, bpmp->dev, " flags: %lx %s\n", info->flags, flags); + dev_printk(level, bpmp->dev, " parents: %u\n", info->num_parents); + + for (i = 0; i < info->num_parents; i++) + dev_printk(level, bpmp->dev, " %03u\n", info->parents[i]); +} + +static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp, + struct tegra_bpmp_clk_info **clocksp) +{ + struct tegra_bpmp_clk_info *clocks; + unsigned int max_id, id, count = 0; + unsigned int holes = 0; + int err; + + err = tegra_bpmp_clk_get_max_id(bpmp); + if (err < 0) + return err; + + max_id = err; + + dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id); + + clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL); + if (!clocks) + return -ENOMEM; + + for (id = 0; id <= max_id; id++) { + struct tegra_bpmp_clk_info *info = &clocks[count]; + + err = tegra_bpmp_clk_get_info(bpmp, id, info); + if (err < 0) + continue; + + if (info->num_parents >= U8_MAX) { + dev_err(bpmp->dev, + "clock %u has too many parents (%u, max: %u)\n", + id, info->num_parents, U8_MAX); + continue; + } + + /* clock not exposed by BPMP */ + if (info->name[0] == '\0') { + holes++; + continue; + } + + info->id = id; + count++; + + if (TEGRA_BPMP_DUMP_CLOCK_INFO) + tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info); + } + + dev_dbg(bpmp->dev, "holes: %u\n", holes); + *clocksp = clocks; + + return count; +} + +static unsigned int +tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks, + unsigned int num_clocks, unsigned int id) +{ + unsigned int i; + + for (i = 0; i < num_clocks; i++) + if (clocks[i].id == id) + return i; + + return UINT_MAX; +} + +static const struct tegra_bpmp_clk_info * +tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks, + unsigned int num_clocks, unsigned int id) +{ + unsigned int i; + + i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id); + + if (i < num_clocks) + return &clocks[i]; + + return NULL; +} + +static struct tegra_bpmp_clk * +tegra_bpmp_clk_register(struct tegra_bpmp *bpmp, + const struct tegra_bpmp_clk_info *info, + const struct tegra_bpmp_clk_info *clocks, + unsigned int num_clocks) +{ + struct tegra_bpmp_clk *clk; + struct clk_init_data init; + const char **parents; + unsigned int i; + int err; + + clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL); + if (!clk) + return ERR_PTR(-ENOMEM); + + clk->id = info->id; + clk->bpmp = bpmp; + + clk->parents = devm_kcalloc(bpmp->dev, info->num_parents, + sizeof(*clk->parents), GFP_KERNEL); + if (!clk->parents) + return ERR_PTR(-ENOMEM); + + clk->num_parents = info->num_parents; + + /* hardware clock initialization */ + memset(&init, 0, sizeof(init)); + init.name = info->name; + clk->hw.init = &init; + + if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) { + if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) + init.ops = &tegra_bpmp_clk_mux_rate_ops; + else + init.ops = &tegra_bpmp_clk_mux_ops; + } else { + if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) + init.ops = &tegra_bpmp_clk_rate_ops; + else + init.ops = &tegra_bpmp_clk_gate_ops; + } + + init.num_parents = info->num_parents; + + parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL); + if (!parents) + return ERR_PTR(-ENOMEM); + + for (i = 0; i < info->num_parents; i++) { + const struct tegra_bpmp_clk_info *parent; + + /* keep a private copy of the ID to parent index map */ + clk->parents[i] = info->parents[i]; + + parent = tegra_bpmp_clk_find(clocks, num_clocks, + info->parents[i]); + if (!parent) { + dev_err(bpmp->dev, "no parent %u found for %u\n", + info->parents[i], info->id); + continue; + } + + parents[i] = parent->name; + } + + init.parent_names = parents; + + err = devm_clk_hw_register(bpmp->dev, &clk->hw); + + kfree(parents); + + if (err < 0) + return ERR_PTR(err); + + return clk; +} + +static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp, + struct tegra_bpmp_clk_info *infos, + unsigned int i, + unsigned int count) +{ + unsigned int j; + struct tegra_bpmp_clk_info *info; + struct tegra_bpmp_clk *clk; + + if (bpmp->clocks[i]) { + /* already registered */ + return; + } + + info = &infos[i]; + for (j = 0; j < info->num_parents; ++j) { + unsigned int p_id = info->parents[j]; + unsigned int p_i = tegra_bpmp_clk_id_to_index(infos, count, + p_id); + if (p_i < count) + tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count); + } + + clk = tegra_bpmp_clk_register(bpmp, info, infos, count); + if (IS_ERR(clk)) { + dev_err(bpmp->dev, + "failed to register clock %u (%s): %ld\n", + info->id, info->name, PTR_ERR(clk)); + /* intentionally store the error pointer to + * bpmp->clocks[i] to avoid re-attempting the + * registration later + */ + } + + bpmp->clocks[i] = clk; +} + +static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp, + struct tegra_bpmp_clk_info *infos, + unsigned int count) +{ + unsigned int i; + + bpmp->num_clocks = count; + + bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(struct tegra_bpmp_clk), GFP_KERNEL); + if (!bpmp->clocks) + return -ENOMEM; + + for (i = 0; i < count; i++) { + tegra_bpmp_register_clocks_one(bpmp, infos, i, count); + } + + return 0; +} + +static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp) +{ + unsigned int i; + + for (i = 0; i < bpmp->num_clocks; i++) + clk_hw_unregister(&bpmp->clocks[i]->hw); +} + +static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec, + void *data) +{ + unsigned int id = clkspec->args[0], i; + struct tegra_bpmp *bpmp = data; + + for (i = 0; i < bpmp->num_clocks; i++) { + struct tegra_bpmp_clk *clk = bpmp->clocks[i]; + + if (!clk) + continue; + + if (clk->id == id) + return &clk->hw; + } + + return NULL; +} + +int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp) +{ + struct tegra_bpmp_clk_info *clocks; + unsigned int count; + int err; + + err = tegra_bpmp_probe_clocks(bpmp, &clocks); + if (err < 0) + return err; + + count = err; + + dev_dbg(bpmp->dev, "%u clocks probed\n", count); + + err = tegra_bpmp_register_clocks(bpmp, clocks, count); + if (err < 0) + goto free; + + err = of_clk_add_hw_provider(bpmp->dev->of_node, + tegra_bpmp_clk_of_xlate, + bpmp); + if (err < 0) { + tegra_bpmp_unregister_clocks(bpmp); + goto free; + } + +free: + kfree(clocks); + return err; +} diff --git a/drivers/firmware/tegra/bpmp-debugfs.c b/drivers/firmware/tegra/bpmp-debugfs.c new file mode 100644 index 00000000..0c440afd --- /dev/null +++ b/drivers/firmware/tegra/bpmp-debugfs.c @@ -0,0 +1,811 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2017, NVIDIA CORPORATION. All rights reserved. + */ +#include +#include +#include +#include + +#include +#include + +static DEFINE_MUTEX(bpmp_debug_lock); + +struct seqbuf { + char *buf; + size_t pos; + size_t size; +}; + +static void seqbuf_init(struct seqbuf *seqbuf, void *buf, size_t size) +{ + seqbuf->buf = buf; + seqbuf->size = size; + seqbuf->pos = 0; +} + +static size_t seqbuf_avail(struct seqbuf *seqbuf) +{ + return seqbuf->pos < seqbuf->size ? seqbuf->size - seqbuf->pos : 0; +} + +static size_t seqbuf_status(struct seqbuf *seqbuf) +{ + return seqbuf->pos <= seqbuf->size ? 0 : -EOVERFLOW; +} + +static int seqbuf_eof(struct seqbuf *seqbuf) +{ + return seqbuf->pos >= seqbuf->size; +} + +static int seqbuf_read(struct seqbuf *seqbuf, void *buf, size_t nbyte) +{ + nbyte = min(nbyte, seqbuf_avail(seqbuf)); + memcpy(buf, seqbuf->buf + seqbuf->pos, nbyte); + seqbuf->pos += nbyte; + return seqbuf_status(seqbuf); +} + +static int seqbuf_read_u32(struct seqbuf *seqbuf, uint32_t *v) +{ + int err; + + err = seqbuf_read(seqbuf, v, 4); + *v = le32_to_cpu(*v); + return err; +} + +static int seqbuf_read_str(struct seqbuf *seqbuf, const char **str) +{ + *str = seqbuf->buf + seqbuf->pos; + seqbuf->pos += strnlen(*str, seqbuf_avail(seqbuf)); + seqbuf->pos++; + return seqbuf_status(seqbuf); +} + +static void seqbuf_seek(struct seqbuf *seqbuf, ssize_t offset) +{ + seqbuf->pos += offset; +} + +/* map filename in Linux debugfs to corresponding entry in BPMP */ +static const char *get_filename(struct tegra_bpmp *bpmp, + const struct file *file, char *buf, int size) +{ + const char *root_path, *filename = NULL; + char *root_path_buf; + size_t root_len; + size_t root_path_buf_len = 512; + + root_path_buf = kzalloc(root_path_buf_len, GFP_KERNEL); + if (!root_path_buf) + goto out; + + root_path = dentry_path(bpmp->debugfs_mirror, root_path_buf, + root_path_buf_len); + if (IS_ERR(root_path)) + goto out; + + root_len = strlen(root_path); + + filename = dentry_path(file->f_path.dentry, buf, size); + if (IS_ERR(filename)) { + filename = NULL; + goto out; + } + + if (strlen(filename) < root_len || strncmp(filename, root_path, root_len)) { + filename = NULL; + goto out; + } + + filename += root_len; + +out: + kfree(root_path_buf); + return filename; +} + +static int mrq_debug_open(struct tegra_bpmp *bpmp, const char *name, + uint32_t *fd, uint32_t *len, bool write) +{ + struct mrq_debug_request req = { + .cmd = cpu_to_le32(write ? CMD_DEBUG_OPEN_WO : CMD_DEBUG_OPEN_RO), + }; + struct mrq_debug_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUG, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + ssize_t sz_name; + int err = 0; + + sz_name = strscpy(req.fop.name, name, sizeof(req.fop.name)); + if (sz_name < 0) { + pr_err("File name too large: %s\n", name); + return -EINVAL; + } + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + *len = resp.fop.datalen; + *fd = resp.fop.fd; + + return 0; +} + +static int mrq_debug_close(struct tegra_bpmp *bpmp, uint32_t fd) +{ + struct mrq_debug_request req = { + .cmd = cpu_to_le32(CMD_DEBUG_CLOSE), + .frd = { + .fd = fd, + }, + }; + struct mrq_debug_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUG, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + int err = 0; + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + return 0; +} + +static int mrq_debug_read(struct tegra_bpmp *bpmp, const char *name, + char *data, size_t sz_data, uint32_t *nbytes) +{ + struct mrq_debug_request req = { + .cmd = cpu_to_le32(CMD_DEBUG_READ), + }; + struct mrq_debug_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUG, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + uint32_t fd = 0, len = 0; + int remaining, err; + + mutex_lock(&bpmp_debug_lock); + err = mrq_debug_open(bpmp, name, &fd, &len, 0); + if (err) + goto out; + + if (len > sz_data) { + err = -EFBIG; + goto close; + } + + req.frd.fd = fd; + remaining = len; + + while (remaining > 0) { + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) { + goto close; + } else if (msg.rx.ret < 0) { + err = -EINVAL; + goto close; + } + + if (resp.frd.readlen > remaining) { + pr_err("%s: read data length invalid\n", __func__); + err = -EINVAL; + goto close; + } + + memcpy(data, resp.frd.data, resp.frd.readlen); + data += resp.frd.readlen; + remaining -= resp.frd.readlen; + } + + *nbytes = len; + +close: + err = mrq_debug_close(bpmp, fd); +out: + mutex_unlock(&bpmp_debug_lock); + return err; +} + +static int mrq_debug_write(struct tegra_bpmp *bpmp, const char *name, + uint8_t *data, size_t sz_data) +{ + struct mrq_debug_request req = { + .cmd = cpu_to_le32(CMD_DEBUG_WRITE) + }; + struct mrq_debug_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUG, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + uint32_t fd = 0, len = 0; + size_t remaining; + int err; + + mutex_lock(&bpmp_debug_lock); + err = mrq_debug_open(bpmp, name, &fd, &len, 1); + if (err) + goto out; + + if (sz_data > len) { + err = -EINVAL; + goto close; + } + + req.fwr.fd = fd; + remaining = sz_data; + + while (remaining > 0) { + len = min(remaining, sizeof(req.fwr.data)); + memcpy(req.fwr.data, data, len); + req.fwr.datalen = len; + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) { + goto close; + } else if (msg.rx.ret < 0) { + err = -EINVAL; + goto close; + } + + data += req.fwr.datalen; + remaining -= req.fwr.datalen; + } + +close: + err = mrq_debug_close(bpmp, fd); +out: + mutex_unlock(&bpmp_debug_lock); + return err; +} + +static int bpmp_debug_show(struct seq_file *m, void *p) +{ + struct file *file = m->private; + struct inode *inode = file_inode(file); + struct tegra_bpmp *bpmp = inode->i_private; + char fnamebuf[256]; + const char *filename; + struct mrq_debug_request req = { + .cmd = cpu_to_le32(CMD_DEBUG_READ), + }; + struct mrq_debug_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUG, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + uint32_t fd = 0, len = 0; + int remaining, err; + + filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf)); + if (!filename) + return -ENOENT; + + mutex_lock(&bpmp_debug_lock); + err = mrq_debug_open(bpmp, filename, &fd, &len, 0); + if (err) + goto out; + + req.frd.fd = fd; + remaining = len; + + while (remaining > 0) { + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) { + goto close; + } else if (msg.rx.ret < 0) { + err = -EINVAL; + goto close; + } + + if (resp.frd.readlen > remaining) { + pr_err("%s: read data length invalid\n", __func__); + err = -EINVAL; + goto close; + } + + seq_write(m, resp.frd.data, resp.frd.readlen); + remaining -= resp.frd.readlen; + } + +close: + err = mrq_debug_close(bpmp, fd); +out: + mutex_unlock(&bpmp_debug_lock); + return err; +} + +static ssize_t bpmp_debug_store(struct file *file, const char __user *buf, + size_t count, loff_t *f_pos) +{ + struct inode *inode = file_inode(file); + struct tegra_bpmp *bpmp = inode->i_private; + char *databuf = NULL; + char fnamebuf[256]; + const char *filename; + ssize_t err; + + filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf)); + if (!filename) + return -ENOENT; + + databuf = kmalloc(count, GFP_KERNEL); + if (!databuf) + return -ENOMEM; + + if (copy_from_user(databuf, buf, count)) { + err = -EFAULT; + goto free_ret; + } + + err = mrq_debug_write(bpmp, filename, databuf, count); + +free_ret: + kfree(databuf); + + return err ?: count; +} + +static int bpmp_debug_open(struct inode *inode, struct file *file) +{ + return single_open_size(file, bpmp_debug_show, file, SZ_256K); +} + +static const struct file_operations bpmp_debug_fops = { + .open = bpmp_debug_open, + .read = seq_read, + .llseek = seq_lseek, + .write = bpmp_debug_store, + .release = single_release, +}; + +static int bpmp_populate_debugfs_inband(struct tegra_bpmp *bpmp, + struct dentry *parent, + char *ppath) +{ + const size_t pathlen = SZ_256; + const size_t bufsize = SZ_16K; + uint32_t dsize, attrs = 0; + struct dentry *dentry; + struct seqbuf seqbuf; + char *buf, *pathbuf; + const char *name; + int err = 0; + + if (!bpmp || !parent || !ppath) + return -EINVAL; + + buf = kmalloc(bufsize, GFP_KERNEL); + if (!buf) + return -ENOMEM; + + pathbuf = kzalloc(pathlen, GFP_KERNEL); + if (!pathbuf) { + kfree(buf); + return -ENOMEM; + } + + err = mrq_debug_read(bpmp, ppath, buf, bufsize, &dsize); + if (err) + goto out; + + seqbuf_init(&seqbuf, buf, dsize); + + while (!seqbuf_eof(&seqbuf)) { + err = seqbuf_read_u32(&seqbuf, &attrs); + if (err) + goto out; + + err = seqbuf_read_str(&seqbuf, &name); + if (err < 0) + goto out; + + if (attrs & DEBUGFS_S_ISDIR) { + size_t len; + + dentry = debugfs_create_dir(name, parent); + if (IS_ERR(dentry)) { + err = PTR_ERR(dentry); + goto out; + } + + len = snprintf(pathbuf, pathlen, "%s%s/", ppath, name); + if (len >= pathlen) { + err = -EINVAL; + goto out; + } + + err = bpmp_populate_debugfs_inband(bpmp, dentry, + pathbuf); + if (err < 0) + goto out; + } else { + umode_t mode; + + mode = attrs & DEBUGFS_S_IRUSR ? 0400 : 0; + mode |= attrs & DEBUGFS_S_IWUSR ? 0200 : 0; + dentry = debugfs_create_file(name, mode, parent, bpmp, + &bpmp_debug_fops); + if (IS_ERR(dentry)) { + err = -ENOMEM; + goto out; + } + } + } + +out: + kfree(pathbuf); + kfree(buf); + + return err; +} + +static int mrq_debugfs_read(struct tegra_bpmp *bpmp, + dma_addr_t name, size_t sz_name, + dma_addr_t data, size_t sz_data, + size_t *nbytes) +{ + struct mrq_debugfs_request req = { + .cmd = cpu_to_le32(CMD_DEBUGFS_READ), + .fop = { + .fnameaddr = cpu_to_le32((uint32_t)name), + .fnamelen = cpu_to_le32((uint32_t)sz_name), + .dataaddr = cpu_to_le32((uint32_t)data), + .datalen = cpu_to_le32((uint32_t)sz_data), + }, + }; + struct mrq_debugfs_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUGFS, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + int err; + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + *nbytes = (size_t)resp.fop.nbytes; + + return 0; +} + +static int mrq_debugfs_write(struct tegra_bpmp *bpmp, + dma_addr_t name, size_t sz_name, + dma_addr_t data, size_t sz_data) +{ + const struct mrq_debugfs_request req = { + .cmd = cpu_to_le32(CMD_DEBUGFS_WRITE), + .fop = { + .fnameaddr = cpu_to_le32((uint32_t)name), + .fnamelen = cpu_to_le32((uint32_t)sz_name), + .dataaddr = cpu_to_le32((uint32_t)data), + .datalen = cpu_to_le32((uint32_t)sz_data), + }, + }; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUGFS, + .tx = { + .data = &req, + .size = sizeof(req), + }, + }; + + return tegra_bpmp_transfer(bpmp, &msg); +} + +static int mrq_debugfs_dumpdir(struct tegra_bpmp *bpmp, dma_addr_t addr, + size_t size, size_t *nbytes) +{ + const struct mrq_debugfs_request req = { + .cmd = cpu_to_le32(CMD_DEBUGFS_DUMPDIR), + .dumpdir = { + .dataaddr = cpu_to_le32((uint32_t)addr), + .datalen = cpu_to_le32((uint32_t)size), + }, + }; + struct mrq_debugfs_response resp; + struct tegra_bpmp_message msg = { + .mrq = MRQ_DEBUGFS, + .tx = { + .data = &req, + .size = sizeof(req), + }, + .rx = { + .data = &resp, + .size = sizeof(resp), + }, + }; + int err; + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + *nbytes = (size_t)resp.dumpdir.nbytes; + + return 0; +} + +static int debugfs_show(struct seq_file *m, void *p) +{ + struct file *file = m->private; + struct inode *inode = file_inode(file); + struct tegra_bpmp *bpmp = inode->i_private; + const size_t datasize = m->size; + const size_t namesize = SZ_256; + void *datavirt, *namevirt; + dma_addr_t dataphys, namephys; + char buf[256]; + const char *filename; + size_t len, nbytes; + int err; + + filename = get_filename(bpmp, file, buf, sizeof(buf)); + if (!filename) + return -ENOENT; + + namevirt = dma_alloc_coherent(bpmp->dev, namesize, &namephys, + GFP_KERNEL | GFP_DMA32); + if (!namevirt) + return -ENOMEM; + + datavirt = dma_alloc_coherent(bpmp->dev, datasize, &dataphys, + GFP_KERNEL | GFP_DMA32); + if (!datavirt) { + err = -ENOMEM; + goto free_namebuf; + } + + len = strlen(filename); + strncpy(namevirt, filename, namesize); + + err = mrq_debugfs_read(bpmp, namephys, len, dataphys, datasize, + &nbytes); + + if (!err) + seq_write(m, datavirt, nbytes); + + dma_free_coherent(bpmp->dev, datasize, datavirt, dataphys); +free_namebuf: + dma_free_coherent(bpmp->dev, namesize, namevirt, namephys); + + return err; +} + +static int debugfs_open(struct inode *inode, struct file *file) +{ + return single_open_size(file, debugfs_show, file, SZ_128K); +} + +static ssize_t debugfs_store(struct file *file, const char __user *buf, + size_t count, loff_t *f_pos) +{ + struct inode *inode = file_inode(file); + struct tegra_bpmp *bpmp = inode->i_private; + const size_t datasize = count; + const size_t namesize = SZ_256; + void *datavirt, *namevirt; + dma_addr_t dataphys, namephys; + char fnamebuf[256]; + const char *filename; + size_t len; + int err; + + filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf)); + if (!filename) + return -ENOENT; + + namevirt = dma_alloc_coherent(bpmp->dev, namesize, &namephys, + GFP_KERNEL | GFP_DMA32); + if (!namevirt) + return -ENOMEM; + + datavirt = dma_alloc_coherent(bpmp->dev, datasize, &dataphys, + GFP_KERNEL | GFP_DMA32); + if (!datavirt) { + err = -ENOMEM; + goto free_namebuf; + } + + len = strlen(filename); + strncpy(namevirt, filename, namesize); + + if (copy_from_user(datavirt, buf, count)) { + err = -EFAULT; + goto free_databuf; + } + + err = mrq_debugfs_write(bpmp, namephys, len, dataphys, + count); + +free_databuf: + dma_free_coherent(bpmp->dev, datasize, datavirt, dataphys); +free_namebuf: + dma_free_coherent(bpmp->dev, namesize, namevirt, namephys); + + return err ?: count; +} + +static const struct file_operations debugfs_fops = { + .open = debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .write = debugfs_store, + .release = single_release, +}; + +static int bpmp_populate_dir(struct tegra_bpmp *bpmp, struct seqbuf *seqbuf, + struct dentry *parent, uint32_t depth) +{ + int err; + uint32_t d, t; + const char *name; + struct dentry *dentry; + + while (!seqbuf_eof(seqbuf)) { + err = seqbuf_read_u32(seqbuf, &d); + if (err < 0) + return err; + + if (d < depth) { + seqbuf_seek(seqbuf, -4); + /* go up a level */ + return 0; + } else if (d != depth) { + /* malformed data received from BPMP */ + return -EIO; + } + + err = seqbuf_read_u32(seqbuf, &t); + if (err < 0) + return err; + err = seqbuf_read_str(seqbuf, &name); + if (err < 0) + return err; + + if (t & DEBUGFS_S_ISDIR) { + dentry = debugfs_create_dir(name, parent); + if (IS_ERR(dentry)) + return -ENOMEM; + err = bpmp_populate_dir(bpmp, seqbuf, dentry, depth+1); + if (err < 0) + return err; + } else { + umode_t mode; + + mode = t & DEBUGFS_S_IRUSR ? S_IRUSR : 0; + mode |= t & DEBUGFS_S_IWUSR ? S_IWUSR : 0; + dentry = debugfs_create_file(name, mode, + parent, bpmp, + &debugfs_fops); + if (IS_ERR(dentry)) + return -ENOMEM; + } + } + + return 0; +} + +static int bpmp_populate_debugfs_shmem(struct tegra_bpmp *bpmp) +{ + struct seqbuf seqbuf; + const size_t sz = SZ_512K; + dma_addr_t phys; + size_t nbytes; + void *virt; + int err; + + virt = dma_alloc_coherent(bpmp->dev, sz, &phys, + GFP_KERNEL | GFP_DMA32); + if (!virt) + return -ENOMEM; + + err = mrq_debugfs_dumpdir(bpmp, phys, sz, &nbytes); + if (err < 0) { + goto free; + } else if (nbytes > sz) { + err = -EINVAL; + goto free; + } + + seqbuf_init(&seqbuf, virt, nbytes); + err = bpmp_populate_dir(bpmp, &seqbuf, bpmp->debugfs_mirror, 0); +free: + dma_free_coherent(bpmp->dev, sz, virt, phys); + + return err; +} + +int tegra_bpmp_init_debugfs(struct tegra_bpmp *bpmp) +{ + struct dentry *root; + bool inband; + int err; + + inband = tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUG); + + if (!inband && !tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUGFS)) + return 0; + + root = debugfs_create_dir("bpmp", NULL); + if (IS_ERR(root)) + return -ENOMEM; + + bpmp->debugfs_mirror = debugfs_create_dir("debug", root); + if (IS_ERR(bpmp->debugfs_mirror)) { + err = -ENOMEM; + goto out; + } + + if (inband) + err = bpmp_populate_debugfs_inband(bpmp, bpmp->debugfs_mirror, + "/"); + else + err = bpmp_populate_debugfs_shmem(bpmp); + +out: + if (err < 0) + debugfs_remove_recursive(root); + + return err; +} diff --git a/drivers/reset/tegra/reset-bpmp.c b/drivers/reset/tegra/reset-bpmp.c new file mode 100644 index 00000000..4c5bba52 --- /dev/null +++ b/drivers/reset/tegra/reset-bpmp.c @@ -0,0 +1,75 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (C) 2016 NVIDIA Corporation + */ + +#include + +#include +#include + +static struct tegra_bpmp *to_tegra_bpmp(struct reset_controller_dev *rstc) +{ + return container_of(rstc, struct tegra_bpmp, rstc); +} + +static int tegra_bpmp_reset_common(struct reset_controller_dev *rstc, + enum mrq_reset_commands command, + unsigned int id) +{ + struct tegra_bpmp *bpmp = to_tegra_bpmp(rstc); + struct mrq_reset_request request; + struct tegra_bpmp_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd = command; + request.reset_id = id; + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_RESET; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err) + return err; + if (msg.rx.ret) + return -EINVAL; + + return 0; +} + +static int tegra_bpmp_reset_module(struct reset_controller_dev *rstc, + unsigned long id) +{ + return tegra_bpmp_reset_common(rstc, CMD_RESET_MODULE, id); +} + +static int tegra_bpmp_reset_assert(struct reset_controller_dev *rstc, + unsigned long id) +{ + return tegra_bpmp_reset_common(rstc, CMD_RESET_ASSERT, id); +} + +static int tegra_bpmp_reset_deassert(struct reset_controller_dev *rstc, + unsigned long id) +{ + return tegra_bpmp_reset_common(rstc, CMD_RESET_DEASSERT, id); +} + +static const struct reset_control_ops tegra_bpmp_reset_ops = { + .reset = tegra_bpmp_reset_module, + .assert = tegra_bpmp_reset_assert, + .deassert = tegra_bpmp_reset_deassert, +}; + +int tegra_bpmp_init_resets(struct tegra_bpmp *bpmp) +{ + bpmp->rstc.ops = &tegra_bpmp_reset_ops; + bpmp->rstc.owner = THIS_MODULE; + bpmp->rstc.of_node = bpmp->dev->of_node; + bpmp->rstc.nr_resets = bpmp->soc->num_resets; + + return devm_reset_controller_register(bpmp->dev, &bpmp->rstc); +} diff --git a/drivers/soc/tegra/powergate-bpmp.c b/drivers/soc/tegra/powergate-bpmp.c new file mode 100644 index 00000000..8eaf50d0 --- /dev/null +++ b/drivers/soc/tegra/powergate-bpmp.c @@ -0,0 +1,361 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2016-2017, NVIDIA CORPORATION. All rights reserved + */ + +#include +#include +#include +#include + +#include +#include + +struct tegra_powergate_info { + unsigned int id; + char *name; +}; + +struct tegra_powergate { + struct generic_pm_domain genpd; + struct tegra_bpmp *bpmp; + unsigned int id; +}; + +static inline struct tegra_powergate * +to_tegra_powergate(struct generic_pm_domain *genpd) +{ + return container_of(genpd, struct tegra_powergate, genpd); +} + +static int tegra_bpmp_powergate_set_state(struct tegra_bpmp *bpmp, + unsigned int id, u32 state) +{ + struct mrq_pg_request request; + struct tegra_bpmp_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd = CMD_PG_SET_STATE; + request.id = id; + request.set_state.state = state; + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_PG; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + return 0; +} + +static int tegra_bpmp_powergate_get_state(struct tegra_bpmp *bpmp, + unsigned int id) +{ + struct mrq_pg_response response; + struct mrq_pg_request request; + struct tegra_bpmp_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd = CMD_PG_GET_STATE; + request.id = id; + + memset(&response, 0, sizeof(response)); + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_PG; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return PG_STATE_OFF; + else if (msg.rx.ret < 0) + return -EINVAL; + + return response.get_state.state; +} + +static int tegra_bpmp_powergate_get_max_id(struct tegra_bpmp *bpmp) +{ + struct mrq_pg_response response; + struct mrq_pg_request request; + struct tegra_bpmp_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd = CMD_PG_GET_MAX_ID; + + memset(&response, 0, sizeof(response)); + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_PG; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0) + return err; + else if (msg.rx.ret < 0) + return -EINVAL; + + return response.get_max_id.max_id; +} + +static char *tegra_bpmp_powergate_get_name(struct tegra_bpmp *bpmp, + unsigned int id) +{ + struct mrq_pg_response response; + struct mrq_pg_request request; + struct tegra_bpmp_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.cmd = CMD_PG_GET_NAME; + request.id = id; + + memset(&response, 0, sizeof(response)); + + memset(&msg, 0, sizeof(msg)); + msg.mrq = MRQ_PG; + msg.tx.data = &request; + msg.tx.size = sizeof(request); + msg.rx.data = &response; + msg.rx.size = sizeof(response); + + err = tegra_bpmp_transfer(bpmp, &msg); + if (err < 0 || msg.rx.ret < 0) + return NULL; + + return kstrdup(response.get_name.name, GFP_KERNEL); +} + +static inline bool tegra_bpmp_powergate_is_powered(struct tegra_bpmp *bpmp, + unsigned int id) +{ + return tegra_bpmp_powergate_get_state(bpmp, id) != PG_STATE_OFF; +} + +static int tegra_powergate_power_on(struct generic_pm_domain *domain) +{ + struct tegra_powergate *powergate = to_tegra_powergate(domain); + struct tegra_bpmp *bpmp = powergate->bpmp; + + return tegra_bpmp_powergate_set_state(bpmp, powergate->id, + PG_STATE_ON); +} + +static int tegra_powergate_power_off(struct generic_pm_domain *domain) +{ + struct tegra_powergate *powergate = to_tegra_powergate(domain); + struct tegra_bpmp *bpmp = powergate->bpmp; + + return tegra_bpmp_powergate_set_state(bpmp, powergate->id, + PG_STATE_OFF); +} + +static struct tegra_powergate * +tegra_powergate_add(struct tegra_bpmp *bpmp, + const struct tegra_powergate_info *info) +{ + struct tegra_powergate *powergate; + bool off; + int err; + + off = !tegra_bpmp_powergate_is_powered(bpmp, info->id); + + powergate = devm_kzalloc(bpmp->dev, sizeof(*powergate), GFP_KERNEL); + if (!powergate) + return ERR_PTR(-ENOMEM); + + powergate->id = info->id; + powergate->bpmp = bpmp; + + powergate->genpd.name = kstrdup(info->name, GFP_KERNEL); + powergate->genpd.power_on = tegra_powergate_power_on; + powergate->genpd.power_off = tegra_powergate_power_off; + + err = pm_genpd_init(&powergate->genpd, NULL, off); + if (err < 0) { + kfree(powergate->genpd.name); + return ERR_PTR(err); + } + + return powergate; +} + +static void tegra_powergate_remove(struct tegra_powergate *powergate) +{ + struct generic_pm_domain *genpd = &powergate->genpd; + struct tegra_bpmp *bpmp = powergate->bpmp; + int err; + + err = pm_genpd_remove(genpd); + if (err < 0) + dev_err(bpmp->dev, "failed to remove power domain %s: %d\n", + genpd->name, err); + + kfree(genpd->name); +} + +static int +tegra_bpmp_probe_powergates(struct tegra_bpmp *bpmp, + struct tegra_powergate_info **powergatesp) +{ + struct tegra_powergate_info *powergates; + unsigned int max_id, id, count = 0; + unsigned int num_holes = 0; + int err; + + err = tegra_bpmp_powergate_get_max_id(bpmp); + if (err < 0) + return err; + + max_id = err; + + dev_dbg(bpmp->dev, "maximum powergate ID: %u\n", max_id); + + powergates = kcalloc(max_id + 1, sizeof(*powergates), GFP_KERNEL); + if (!powergates) + return -ENOMEM; + + for (id = 0; id <= max_id; id++) { + struct tegra_powergate_info *info = &powergates[count]; + + info->name = tegra_bpmp_powergate_get_name(bpmp, id); + if (!info->name || info->name[0] == '\0') { + num_holes++; + continue; + } + + info->id = id; + count++; + } + + dev_dbg(bpmp->dev, "holes: %u\n", num_holes); + + *powergatesp = powergates; + + return count; +} + +static int tegra_bpmp_add_powergates(struct tegra_bpmp *bpmp, + struct tegra_powergate_info *powergates, + unsigned int count) +{ + struct genpd_onecell_data *genpd = &bpmp->genpd; + struct generic_pm_domain **domains; + struct tegra_powergate *powergate; + unsigned int i; + int err; + + domains = kcalloc(count, sizeof(*domains), GFP_KERNEL); + if (!domains) + return -ENOMEM; + + for (i = 0; i < count; i++) { + powergate = tegra_powergate_add(bpmp, &powergates[i]); + if (IS_ERR(powergate)) { + err = PTR_ERR(powergate); + goto remove; + } + + dev_dbg(bpmp->dev, "added power domain %s\n", + powergate->genpd.name); + domains[i] = &powergate->genpd; + } + + genpd->num_domains = count; + genpd->domains = domains; + + return 0; + +remove: + while (i--) { + powergate = to_tegra_powergate(domains[i]); + tegra_powergate_remove(powergate); + } + + kfree(genpd->domains); + return err; +} + +static void tegra_bpmp_remove_powergates(struct tegra_bpmp *bpmp) +{ + struct genpd_onecell_data *genpd = &bpmp->genpd; + unsigned int i = genpd->num_domains; + struct tegra_powergate *powergate; + + while (i--) { + dev_dbg(bpmp->dev, "removing power domain %s\n", + genpd->domains[i]->name); + powergate = to_tegra_powergate(genpd->domains[i]); + tegra_powergate_remove(powergate); + } +} + +static struct generic_pm_domain * +tegra_powergate_xlate(struct of_phandle_args *spec, void *data) +{ + struct generic_pm_domain *domain = ERR_PTR(-ENOENT); + struct genpd_onecell_data *genpd = data; + unsigned int i; + + for (i = 0; i < genpd->num_domains; i++) { + struct tegra_powergate *powergate; + + powergate = to_tegra_powergate(genpd->domains[i]); + if (powergate->id == spec->args[0]) { + domain = &powergate->genpd; + break; + } + } + + return domain; +} + +int tegra_bpmp_init_powergates(struct tegra_bpmp *bpmp) +{ + struct device_node *np = bpmp->dev->of_node; + struct tegra_powergate_info *powergates; + struct device *dev = bpmp->dev; + unsigned int count, i; + int err; + + err = tegra_bpmp_probe_powergates(bpmp, &powergates); + if (err < 0) + return err; + + count = err; + + dev_dbg(dev, "%u power domains probed\n", count); + + err = tegra_bpmp_add_powergates(bpmp, powergates, count); + if (err < 0) + goto free; + + bpmp->genpd.xlate = tegra_powergate_xlate; + + err = of_genpd_add_provider_onecell(np, &bpmp->genpd); + if (err < 0) { + dev_err(dev, "failed to add power domain provider: %d\n", err); + tegra_bpmp_remove_powergates(bpmp); + } + +free: + for (i = 0; i < count; i++) + kfree(powergates[i].name); + + kfree(powergates); + return err; +}