193 lines · c
1// SPDX-License-Identifier: GPL-2.02#include <linux/io.h>3#include <linux/clk-provider.h>4#include <linux/slab.h>5#include <linux/of.h>6#include <linux/of_address.h>7 8#include "clk.h"9 10void mmp_clk_init(struct device_node *np, struct mmp_clk_unit *unit,11 int nr_clks)12{13 struct clk **clk_table;14 15 clk_table = kcalloc(nr_clks, sizeof(struct clk *), GFP_KERNEL);16 if (!clk_table)17 return;18 19 unit->clk_table = clk_table;20 unit->nr_clks = nr_clks;21 unit->clk_data.clks = clk_table;22 unit->clk_data.clk_num = nr_clks;23 of_clk_add_provider(np, of_clk_src_onecell_get, &unit->clk_data);24}25 26void mmp_register_fixed_rate_clks(struct mmp_clk_unit *unit,27 struct mmp_param_fixed_rate_clk *clks,28 int size)29{30 int i;31 struct clk *clk;32 33 for (i = 0; i < size; i++) {34 clk = clk_register_fixed_rate(NULL, clks[i].name,35 clks[i].parent_name,36 clks[i].flags,37 clks[i].fixed_rate);38 if (IS_ERR(clk)) {39 pr_err("%s: failed to register clock %s\n",40 __func__, clks[i].name);41 continue;42 }43 if (clks[i].id)44 unit->clk_table[clks[i].id] = clk;45 }46}47 48void mmp_register_fixed_factor_clks(struct mmp_clk_unit *unit,49 struct mmp_param_fixed_factor_clk *clks,50 int size)51{52 struct clk *clk;53 int i;54 55 for (i = 0; i < size; i++) {56 clk = clk_register_fixed_factor(NULL, clks[i].name,57 clks[i].parent_name,58 clks[i].flags, clks[i].mult,59 clks[i].div);60 if (IS_ERR(clk)) {61 pr_err("%s: failed to register clock %s\n",62 __func__, clks[i].name);63 continue;64 }65 if (clks[i].id)66 unit->clk_table[clks[i].id] = clk;67 }68}69 70void mmp_register_general_gate_clks(struct mmp_clk_unit *unit,71 struct mmp_param_general_gate_clk *clks,72 void __iomem *base, int size)73{74 struct clk *clk;75 int i;76 77 for (i = 0; i < size; i++) {78 clk = clk_register_gate(NULL, clks[i].name,79 clks[i].parent_name,80 clks[i].flags,81 base + clks[i].offset,82 clks[i].bit_idx,83 clks[i].gate_flags,84 clks[i].lock);85 86 if (IS_ERR(clk)) {87 pr_err("%s: failed to register clock %s\n",88 __func__, clks[i].name);89 continue;90 }91 if (clks[i].id)92 unit->clk_table[clks[i].id] = clk;93 }94}95 96void mmp_register_gate_clks(struct mmp_clk_unit *unit,97 struct mmp_param_gate_clk *clks,98 void __iomem *base, int size)99{100 struct clk *clk;101 int i;102 103 for (i = 0; i < size; i++) {104 clk = mmp_clk_register_gate(NULL, clks[i].name,105 clks[i].parent_name,106 clks[i].flags,107 base + clks[i].offset,108 clks[i].mask,109 clks[i].val_enable,110 clks[i].val_disable,111 clks[i].gate_flags,112 clks[i].lock);113 114 if (IS_ERR(clk)) {115 pr_err("%s: failed to register clock %s\n",116 __func__, clks[i].name);117 continue;118 }119 if (clks[i].id)120 unit->clk_table[clks[i].id] = clk;121 }122}123 124void mmp_register_mux_clks(struct mmp_clk_unit *unit,125 struct mmp_param_mux_clk *clks,126 void __iomem *base, int size)127{128 struct clk *clk;129 int i;130 131 for (i = 0; i < size; i++) {132 clk = clk_register_mux(NULL, clks[i].name,133 clks[i].parent_name,134 clks[i].num_parents,135 clks[i].flags,136 base + clks[i].offset,137 clks[i].shift,138 clks[i].width,139 clks[i].mux_flags,140 clks[i].lock);141 142 if (IS_ERR(clk)) {143 pr_err("%s: failed to register clock %s\n",144 __func__, clks[i].name);145 continue;146 }147 if (clks[i].id)148 unit->clk_table[clks[i].id] = clk;149 }150}151 152void mmp_register_div_clks(struct mmp_clk_unit *unit,153 struct mmp_param_div_clk *clks,154 void __iomem *base, int size)155{156 struct clk *clk;157 int i;158 159 for (i = 0; i < size; i++) {160 clk = clk_register_divider(NULL, clks[i].name,161 clks[i].parent_name,162 clks[i].flags,163 base + clks[i].offset,164 clks[i].shift,165 clks[i].width,166 clks[i].div_flags,167 clks[i].lock);168 169 if (IS_ERR(clk)) {170 pr_err("%s: failed to register clock %s\n",171 __func__, clks[i].name);172 continue;173 }174 if (clks[i].id)175 unit->clk_table[clks[i].id] = clk;176 }177}178 179void mmp_clk_add(struct mmp_clk_unit *unit, unsigned int id,180 struct clk *clk)181{182 if (IS_ERR_OR_NULL(clk)) {183 pr_err("CLK %d has invalid pointer %p\n", id, clk);184 return;185 }186 if (id >= unit->nr_clks) {187 pr_err("CLK %d is invalid\n", id);188 return;189 }190 191 unit->clk_table[id] = clk;192}193