From patchwork Thu Nov 17 15:47:31 2016 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Thierry Reding X-Patchwork-Id: 9434961 X-Patchwork-Delegate: sboyd@codeaurora.org Return-Path: Received: from mail.wl.linuxfoundation.org (pdx-wl-mail.web.codeaurora.org [172.30.200.125]) by pdx-korg-patchwork.web.codeaurora.org (Postfix) with ESMTP id 4507460469 for ; Thu, 17 Nov 2016 18:00:10 +0000 (UTC) Received: from mail.wl.linuxfoundation.org (localhost [127.0.0.1]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id 365C82960B for ; Thu, 17 Nov 2016 18:00:10 +0000 (UTC) Received: by mail.wl.linuxfoundation.org (Postfix, from userid 486) id 2A11D29689; Thu, 17 Nov 2016 18:00:10 +0000 (UTC) X-Spam-Checker-Version: SpamAssassin 3.3.1 (2010-03-16) on pdx-wl-mail.web.codeaurora.org X-Spam-Level: X-Spam-Status: No, score=-6.8 required=2.0 tests=BAYES_00, DKIM_ADSP_CUSTOM_MED, DKIM_SIGNED, FREEMAIL_FROM, RCVD_IN_DNSWL_HI, T_DKIM_INVALID autolearn=ham version=3.3.1 Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.wl.linuxfoundation.org (Postfix) with ESMTP id CEC8B2960B for ; Thu, 17 Nov 2016 18:00:08 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S933965AbcKQSAH (ORCPT ); Thu, 17 Nov 2016 13:00:07 -0500 Received: from mail-pg0-f68.google.com ([74.125.83.68]:34860 "EHLO mail-pg0-f68.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S933640AbcKQSAG (ORCPT ); Thu, 17 Nov 2016 13:00:06 -0500 Received: by mail-pg0-f68.google.com with SMTP id p66so18172602pga.2; Thu, 17 Nov 2016 10:00:06 -0800 (PST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20120113; h=from:to:cc:subject:date:message-id:in-reply-to:references; bh=vwxu4AlG2OkWU0c44gvqo7qY4J1xG4zoZcrmZA6PXcU=; b=B8ZFIzVC5G382QqAppU8vcn3c3KBLVAO4oaFdSShuc9i7PTwx8OaISib3RWK8DwNv+ 1hapl6WWP9YS9w1hnCCU+MMpDIZqojA7L+1C+UECT53gequdz8Xz9lIS64GamNQwEv64 MReRa9sDd01f3Q8L5xnXzs8boWAIKgsnDf+m3HyyUO7WS4V2RwWHVyRKZ07Ft5JSTD2O TIgH8w5pm++TZL+jVf5ewLgXtcJ2bQo8Avt2jIOanSbQVhxkI8+gq6gfX0AfRTZz8pGU WDitPh1POt/IHWbhKpj3HXOII8qsk5lrznt/ufnu/SXCfbgahvzPuuCcbg1PbgnyjA4o VYqA== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20130820; h=x-gm-message-state:from:to:cc:subject:date:message-id:in-reply-to :references; bh=vwxu4AlG2OkWU0c44gvqo7qY4J1xG4zoZcrmZA6PXcU=; b=F968e2cQjXYaawvzjKBRb4ivXsY7/+R1K/98lmfKDsSR1xpu26UH6twAKxxQxhz1QP 2Op65b81vzITJU/Lh2HIk4f1vk6kiici47Chx8+FO4DsfEszgjNsSd5el+RuyYng2QpA ZE4nQ6SohNWDro6y8I2t/vt/Np8WKgPsCdMSAkVkxzKrSlpRgIoza0Abtg2YO69/BHDU zu10tzr2hOQj5CcXIRTGxTD/zdXYGcBBLnpuu/gM9o4phoLJhkE+8RQS+RIibb3/r3h6 nz7ZZcNdfMSbsP4JKzrhmFRtz7ldrnIwOnO1xIvqMJyAkvZw1YM/+r2WQG/aY13q046W KQGA== X-Gm-Message-State: ABUngvcVP5t0F3Eb8yGsZwpU4uUFHm7SP2mOPEsJIjq8oQVVIEWVwlYyjpRTsQn8CrnN+w== X-Received: by 10.99.145.74 with SMTP id l71mr9006569pge.128.1479397655240; Thu, 17 Nov 2016 07:47:35 -0800 (PST) Received: from localhost (port-24584.pppoe.wtnet.de. [46.59.157.190]) by smtp.gmail.com with ESMTPSA id q12sm8951617pfj.18.2016.11.17.07.47.33 (version=TLS1_2 cipher=ECDHE-RSA-CHACHA20-POLY1305 bits=256/256); Thu, 17 Nov 2016 07:47:33 -0800 (PST) From: Thierry Reding To: Michael Turquette , Stephen Boyd Cc: Stephen Warren , Alexandre Courbot , Jon Hunter , linux-clk@vger.kernel.org, linux-tegra@vger.kernel.org Subject: [PATCH v2] clk: tegra: Add BPMP clock driver Date: Thu, 17 Nov 2016 16:47:31 +0100 Message-Id: <20161117154731.29528-1-thierry.reding@gmail.com> X-Mailer: git-send-email 2.10.2 In-Reply-To: <20161115161129.29722-1-thierry.reding@gmail.com> References: <20161115161129.29722-1-thierry.reding@gmail.com> Sender: linux-clk-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-clk@vger.kernel.org X-Virus-Scanned: ClamAV using ClamSMTP From: Thierry Reding This driver uses the services provided by the BPMP firmware driver to implement a clock driver based on the MRQ_CLK request. This part of the BPMP ABI provides a means to enumerate and control clocks and should allow the driver to work on any chip that supports this ABI. Signed-off-by: Thierry Reding --- Changes in v2: - rename ->prepare() and ->unprepare() implementations for consistency - implement ->is_prepared() instead of ->is_enabled() to avoid the need for atomic operations - rename tegra_bpmp_clk_message.clk member to id - remove a double semi-colon and a stray ampersand - drop extra check for validity of parent index, the core does it already - make struct tegra_bpmp carry an array of struct tegra_bpmp_clk instead of struct clk_hw to simplify some driver code - zero out struct clk_init_data to avoid potentially uninitialized data - use devm_clk_hw_register() instead of clk_register() because we never need the opaque struct clk cookie - rearrange functions so that they appear in the order specified by struct clk_ops drivers/clk/tegra/Kconfig | 4 + drivers/clk/tegra/Makefile | 1 + drivers/clk/tegra/clk-bpmp.c | 620 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 625 insertions(+) create mode 100644 drivers/clk/tegra/clk-bpmp.c diff --git a/drivers/clk/tegra/Kconfig b/drivers/clk/tegra/Kconfig index 1ba30d1e14f2..7ddacae5d0b1 100644 --- a/drivers/clk/tegra/Kconfig +++ b/drivers/clk/tegra/Kconfig @@ -1,3 +1,7 @@ config TEGRA_CLK_EMC def_bool y depends on TEGRA124_EMC + +config CLK_TEGRA_BPMP + def_bool y + depends on TEGRA_BPMP diff --git a/drivers/clk/tegra/Makefile b/drivers/clk/tegra/Makefile index 33fd0938d79e..4be8af28ee61 100644 --- a/drivers/clk/tegra/Makefile +++ b/drivers/clk/tegra/Makefile @@ -22,3 +22,4 @@ obj-$(CONFIG_ARCH_TEGRA_124_SOC) += clk-tegra124-dfll-fcpu.o obj-$(CONFIG_ARCH_TEGRA_132_SOC) += clk-tegra124.o obj-y += cvb.o obj-$(CONFIG_ARCH_TEGRA_210_SOC) += clk-tegra210.o +obj-$(CONFIG_CLK_TEGRA_BPMP) += clk-bpmp.o diff --git a/drivers/clk/tegra/clk-bpmp.c b/drivers/clk/tegra/clk-bpmp.c new file mode 100644 index 000000000000..638ace64033b --- /dev/null +++ b/drivers/clk/tegra/clk-bpmp.c @@ -0,0 +1,620 @@ +/* + * Copyright (C) 2016 NVIDIA Corporation + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#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; + } 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; + + 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; + + return tegra_bpmp_transfer(bpmp, &msg); +} + +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 long tegra_bpmp_clk_round_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_round_rate_response response; + struct cmd_clk_round_rate_request request; + struct tegra_bpmp_clk_message msg; + int err; + + memset(&request, 0, sizeof(request)); + request.rate = rate; + + 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; + + return response.rate; +} + +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 = rate; + + 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, + .round_rate = tegra_bpmp_clk_round_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, + .round_rate = tegra_bpmp_clk_round_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) { + dev_err(bpmp->dev, "failed to query clock %u: %d\n", + id, err); + 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 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; + + for (i = 0; i < num_clocks; i++) + if (clocks[i].id == id) + 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 int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp, + struct tegra_bpmp_clk_info *infos, + unsigned int count) +{ + struct tegra_bpmp_clk *clk; + unsigned int i; + + bpmp->num_clocks = count; + + bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(clk), GFP_KERNEL); + if (!bpmp->clocks) + return -ENOMEM; + + for (i = 0; i < count; i++) { + struct tegra_bpmp_clk_info *info = &infos[i]; + + 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)); + continue; + } + + bpmp->clocks[i] = clk; + } + + 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++) + if (bpmp->clocks[i]->id == id) + return &bpmp->clocks[i]->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; +}