LXR linux/drivers/clk/mediatek/clk-cpumux.c

   1/*
   2 * Copyright (c) 2015 Linaro Ltd.
   3 * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
   4 *
   5 * This program is free software; you can redistribute it and/or modify
   6 * it under the terms of the GNU General Public License version 2 as
   7 * published by the Free Software Foundation.
   8 *
   9 * This program is distributed in the hope that it will be useful,
  10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 * GNU General Public License for more details.
  13 */
  14
  15#include <linux/clk-provider.h>
  16#include <linux/mfd/syscon.h>
  17#include <linux/slab.h>
  18
  19#include "clk-mtk.h"
  20#include "clk-cpumux.h"
  21
  22static inline struct mtk_clk_cpumux *to_mtk_clk_cpumux(struct clk_hw *_hw)
  23{
  24        return container_of(_hw, struct mtk_clk_cpumux, hw);
  25}
  26
  27static u8 clk_cpumux_get_parent(struct clk_hw *hw)
  28{
  29        struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);
  30        unsigned int val;
  31
  32        regmap_read(mux->regmap, mux->reg, &val);
  33
  34        val >>= mux->shift;
  35        val &= mux->mask;
  36
  37        return val;
  38}
  39
  40static int clk_cpumux_set_parent(struct clk_hw *hw, u8 index)
  41{
  42        struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);
  43        u32 mask, val;
  44
  45        val = index << mux->shift;
  46        mask = mux->mask << mux->shift;
  47
  48        return regmap_update_bits(mux->regmap, mux->reg, mask, val);
  49}
  50
  51static const struct clk_ops clk_cpumux_ops = {
  52        .get_parent = clk_cpumux_get_parent,
  53        .set_parent = clk_cpumux_set_parent,
  54};
  55
  56static struct clk __init *
  57mtk_clk_register_cpumux(const struct mtk_composite *mux,
  58                        struct regmap *regmap)
  59{
  60        struct mtk_clk_cpumux *cpumux;
  61        struct clk *clk;
  62        struct clk_init_data init;
  63
  64        cpumux = kzalloc(sizeof(*cpumux), GFP_KERNEL);
  65        if (!cpumux)
  66                return ERR_PTR(-ENOMEM);
  67
  68        init.name = mux->name;
  69        init.ops = &clk_cpumux_ops;
  70        init.parent_names = mux->parent_names;
  71        init.num_parents = mux->num_parents;
  72        init.flags = mux->flags;
  73
  74        cpumux->reg = mux->mux_reg;
  75        cpumux->shift = mux->mux_shift;
  76        cpumux->mask = BIT(mux->mux_width) - 1;
  77        cpumux->regmap = regmap;
  78        cpumux->hw.init = &init;
  79
  80        clk = clk_register(NULL, &cpumux->hw);
  81        if (IS_ERR(clk))
  82                kfree(cpumux);
  83
  84        return clk;
  85}
  86
  87int __init mtk_clk_register_cpumuxes(struct device_node *node,
  88                                     const struct mtk_composite *clks, int num,
  89                                     struct clk_onecell_data *clk_data)
  90{
  91        int i;
  92        struct clk *clk;
  93        struct regmap *regmap;
  94
  95        regmap = syscon_node_to_regmap(node);
  96        if (IS_ERR(regmap)) {
  97                pr_err("Cannot find regmap for %pOF: %ld\n", node,
  98                       PTR_ERR(regmap));
  99                return PTR_ERR(regmap);
 100        }
 101
 102        for (i = 0; i < num; i++) {
 103                const struct mtk_composite *mux = &clks[i];
 104
 105                clk = mtk_clk_register_cpumux(mux, regmap);
 106                if (IS_ERR(clk)) {
 107                        pr_err("Failed to register clk %s: %ld\n",
 108                               mux->name, PTR_ERR(clk));
 109                        continue;
 110                }
 111
 112                clk_data->clks[mux->id] = clk;
 113        }
 114
 115        return 0;
 116}
 117