mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
253 lines
5.8 KiB
253 lines
5.8 KiB
// SPDX-License-Identifier: GPL-2.0-or-later |
|
/* |
|
* Copyright (C) 2013 Boris BREZILLON <[email protected]> |
|
*/ |
|
|
|
#include <linux/clk-provider.h> |
|
#include <linux/clkdev.h> |
|
#include <linux/clk/at91_pmc.h> |
|
#include <linux/of.h> |
|
#include <linux/mfd/syscon.h> |
|
#include <linux/regmap.h> |
|
|
|
#include "pmc.h" |
|
|
|
#define PROG_ID_MAX 7 |
|
|
|
#define PROG_STATUS_MASK(id) (1 << ((id) + 8)) |
|
#define PROG_PRES(layout, pckr) ((pckr >> layout->pres_shift) & layout->pres_mask) |
|
#define PROG_MAX_RM9200_CSS 3 |
|
|
|
struct clk_programmable { |
|
struct clk_hw hw; |
|
struct regmap *regmap; |
|
u32 *mux_table; |
|
u8 id; |
|
const struct clk_programmable_layout *layout; |
|
}; |
|
|
|
#define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) |
|
|
|
static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, |
|
unsigned long parent_rate) |
|
{ |
|
struct clk_programmable *prog = to_clk_programmable(hw); |
|
const struct clk_programmable_layout *layout = prog->layout; |
|
unsigned int pckr; |
|
unsigned long rate; |
|
|
|
regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); |
|
|
|
if (layout->is_pres_direct) |
|
rate = parent_rate / (PROG_PRES(layout, pckr) + 1); |
|
else |
|
rate = parent_rate >> PROG_PRES(layout, pckr); |
|
|
|
return rate; |
|
} |
|
|
|
static int clk_programmable_determine_rate(struct clk_hw *hw, |
|
struct clk_rate_request *req) |
|
{ |
|
struct clk_programmable *prog = to_clk_programmable(hw); |
|
const struct clk_programmable_layout *layout = prog->layout; |
|
struct clk_hw *parent; |
|
long best_rate = -EINVAL; |
|
unsigned long parent_rate; |
|
unsigned long tmp_rate = 0; |
|
int shift; |
|
int i; |
|
|
|
for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
|
parent = clk_hw_get_parent_by_index(hw, i); |
|
if (!parent) |
|
continue; |
|
|
|
parent_rate = clk_hw_get_rate(parent); |
|
if (layout->is_pres_direct) { |
|
for (shift = 0; shift <= layout->pres_mask; shift++) { |
|
tmp_rate = parent_rate / (shift + 1); |
|
if (tmp_rate <= req->rate) |
|
break; |
|
} |
|
} else { |
|
for (shift = 0; shift < layout->pres_mask; shift++) { |
|
tmp_rate = parent_rate >> shift; |
|
if (tmp_rate <= req->rate) |
|
break; |
|
} |
|
} |
|
|
|
if (tmp_rate > req->rate) |
|
continue; |
|
|
|
if (best_rate < 0 || |
|
(req->rate - tmp_rate) < (req->rate - best_rate)) { |
|
best_rate = tmp_rate; |
|
req->best_parent_rate = parent_rate; |
|
req->best_parent_hw = parent; |
|
} |
|
|
|
if (!best_rate) |
|
break; |
|
} |
|
|
|
if (best_rate < 0) |
|
return best_rate; |
|
|
|
req->rate = best_rate; |
|
return 0; |
|
} |
|
|
|
static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) |
|
{ |
|
struct clk_programmable *prog = to_clk_programmable(hw); |
|
const struct clk_programmable_layout *layout = prog->layout; |
|
unsigned int mask = layout->css_mask; |
|
unsigned int pckr = index; |
|
|
|
if (layout->have_slck_mck) |
|
mask |= AT91_PMC_CSSMCK_MCK; |
|
|
|
if (prog->mux_table) |
|
pckr = clk_mux_index_to_val(prog->mux_table, 0, index); |
|
|
|
if (index > layout->css_mask) { |
|
if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck) |
|
return -EINVAL; |
|
|
|
pckr |= AT91_PMC_CSSMCK_MCK; |
|
} |
|
|
|
regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr); |
|
|
|
return 0; |
|
} |
|
|
|
static u8 clk_programmable_get_parent(struct clk_hw *hw) |
|
{ |
|
struct clk_programmable *prog = to_clk_programmable(hw); |
|
const struct clk_programmable_layout *layout = prog->layout; |
|
unsigned int pckr; |
|
u8 ret; |
|
|
|
regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr); |
|
|
|
ret = pckr & layout->css_mask; |
|
|
|
if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret) |
|
ret = PROG_MAX_RM9200_CSS + 1; |
|
|
|
if (prog->mux_table) |
|
ret = clk_mux_val_to_index(&prog->hw, prog->mux_table, 0, ret); |
|
|
|
return ret; |
|
} |
|
|
|
static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, |
|
unsigned long parent_rate) |
|
{ |
|
struct clk_programmable *prog = to_clk_programmable(hw); |
|
const struct clk_programmable_layout *layout = prog->layout; |
|
unsigned long div = parent_rate / rate; |
|
int shift = 0; |
|
|
|
if (!div) |
|
return -EINVAL; |
|
|
|
if (layout->is_pres_direct) { |
|
shift = div - 1; |
|
|
|
if (shift > layout->pres_mask) |
|
return -EINVAL; |
|
} else { |
|
shift = fls(div) - 1; |
|
|
|
if (div != (1 << shift)) |
|
return -EINVAL; |
|
|
|
if (shift >= layout->pres_mask) |
|
return -EINVAL; |
|
} |
|
|
|
regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), |
|
layout->pres_mask << layout->pres_shift, |
|
shift << layout->pres_shift); |
|
|
|
return 0; |
|
} |
|
|
|
static const struct clk_ops programmable_ops = { |
|
.recalc_rate = clk_programmable_recalc_rate, |
|
.determine_rate = clk_programmable_determine_rate, |
|
.get_parent = clk_programmable_get_parent, |
|
.set_parent = clk_programmable_set_parent, |
|
.set_rate = clk_programmable_set_rate, |
|
}; |
|
|
|
struct clk_hw * __init |
|
at91_clk_register_programmable(struct regmap *regmap, |
|
const char *name, const char **parent_names, |
|
u8 num_parents, u8 id, |
|
const struct clk_programmable_layout *layout, |
|
u32 *mux_table) |
|
{ |
|
struct clk_programmable *prog; |
|
struct clk_hw *hw; |
|
struct clk_init_data init; |
|
int ret; |
|
|
|
if (id > PROG_ID_MAX) |
|
return ERR_PTR(-EINVAL); |
|
|
|
prog = kzalloc(sizeof(*prog), GFP_KERNEL); |
|
if (!prog) |
|
return ERR_PTR(-ENOMEM); |
|
|
|
init.name = name; |
|
init.ops = &programmable_ops; |
|
init.parent_names = parent_names; |
|
init.num_parents = num_parents; |
|
init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; |
|
|
|
prog->id = id; |
|
prog->layout = layout; |
|
prog->hw.init = &init; |
|
prog->regmap = regmap; |
|
prog->mux_table = mux_table; |
|
|
|
hw = &prog->hw; |
|
ret = clk_hw_register(NULL, &prog->hw); |
|
if (ret) { |
|
kfree(prog); |
|
hw = ERR_PTR(ret); |
|
} else { |
|
pmc_register_pck(id); |
|
} |
|
|
|
return hw; |
|
} |
|
|
|
const struct clk_programmable_layout at91rm9200_programmable_layout = { |
|
.pres_mask = 0x7, |
|
.pres_shift = 2, |
|
.css_mask = 0x3, |
|
.have_slck_mck = 0, |
|
.is_pres_direct = 0, |
|
}; |
|
|
|
const struct clk_programmable_layout at91sam9g45_programmable_layout = { |
|
.pres_mask = 0x7, |
|
.pres_shift = 2, |
|
.css_mask = 0x3, |
|
.have_slck_mck = 1, |
|
.is_pres_direct = 0, |
|
}; |
|
|
|
const struct clk_programmable_layout at91sam9x5_programmable_layout = { |
|
.pres_mask = 0x7, |
|
.pres_shift = 4, |
|
.css_mask = 0x7, |
|
.have_slck_mck = 0, |
|
.is_pres_direct = 0, |
|
};
|
|
|