mirror of
https://github.com/ARM-software/arm-trusted-firmware.git
synced 2025-04-17 18:14:24 +00:00
feat(nxp-clk): add DFS module enablement
Implement enable and set_module_rate callbacks for DFS modules. Change-Id: Ic9d6034ac04adbabd8fc782aea94ce252439f136 Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
This commit is contained in:
Ghennadi Procopciuc
parent
44ae54af5c
commit
4cd04c50eb
2 changed files with 270 additions and 1 deletions
|
|
@ -9,6 +9,7 @@
|
|||
|
||||
#define FXOSC_BASE_ADDR (0x40050000UL)
|
||||
#define ARMPLL_BASE_ADDR (0x40038000UL)
|
||||
#define ARM_DFS_BASE_ADDR (0x40054000UL)
|
||||
#define CGM1_BASE_ADDR (0x40034000UL)
|
||||
|
||||
/* FXOSC */
|
||||
|
|
@ -83,4 +84,25 @@
|
|||
#define MC_CGM_MUXn_CSS_SWIP BIT_32(16U)
|
||||
#define MC_CGM_MUXn_CSS_SAFE_SW BIT_32(3U)
|
||||
|
||||
/* DFS */
|
||||
#define DFS_PORTSR(DFS_ADDR) ((DFS_ADDR) + 0xCUL)
|
||||
#define DFS_PORTOLSR(DFS_ADDR) ((DFS_ADDR) + 0x10UL)
|
||||
#define DFS_PORTOLSR_LOL(N) (BIT_32(N) & GENMASK_32(5U, 0U))
|
||||
#define DFS_PORTRESET(DFS_ADDR) ((DFS_ADDR) + 0x14UL)
|
||||
#define DFS_PORTRESET_MASK GENMASK_32(5U, 0U)
|
||||
#define DFS_PORTRESET_SET(VAL) (((VAL) & DFS_PORTRESET_MASK))
|
||||
|
||||
#define DFS_CTL(DFS_ADDR) ((DFS_ADDR) + 0x18UL)
|
||||
#define DFS_CTL_RESET BIT_32(1U)
|
||||
|
||||
#define DFS_DVPORTn(DFS_ADDR, PORT) ((DFS_ADDR) + 0x1CUL + ((PORT) * 0x4UL))
|
||||
#define DFS_DVPORTn_MFI_MASK GENMASK_32(15U, 8U)
|
||||
#define DFS_DVPORTn_MFI_SHIFT 8U
|
||||
#define DFS_DVPORTn_MFN_MASK GENMASK_32(7U, 0U)
|
||||
#define DFS_DVPORTn_MFN_SHIFT 0U
|
||||
#define DFS_DVPORTn_MFI(MFI) (((MFI) & DFS_DVPORTn_MFI_MASK) >> DFS_DVPORTn_MFI_SHIFT)
|
||||
#define DFS_DVPORTn_MFN(MFN) (((MFN) & DFS_DVPORTn_MFN_MASK) >> DFS_DVPORTn_MFN_SHIFT)
|
||||
#define DFS_DVPORTn_MFI_SET(VAL) (((VAL) << DFS_DVPORTn_MFI_SHIFT) & DFS_DVPORTn_MFI_MASK)
|
||||
#define DFS_DVPORTn_MFN_SET(VAL) (((VAL) << DFS_DVPORTn_MFN_SHIFT) & DFS_DVPORTn_MFN_MASK)
|
||||
|
||||
#endif /* S32CC_CLK_REGS_H */
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
struct s32cc_clk_drv {
|
||||
uintptr_t fxosc_base;
|
||||
uintptr_t armpll_base;
|
||||
uintptr_t armdfs_base;
|
||||
uintptr_t cgm1_base;
|
||||
};
|
||||
|
||||
|
|
@ -40,6 +41,7 @@ static struct s32cc_clk_drv *get_drv(void)
|
|||
static struct s32cc_clk_drv driver = {
|
||||
.fxosc_base = FXOSC_BASE_ADDR,
|
||||
.armpll_base = ARMPLL_BASE_ADDR,
|
||||
.armdfs_base = ARM_DFS_BASE_ADDR,
|
||||
.cgm1_base = CGM1_BASE_ADDR,
|
||||
};
|
||||
|
||||
|
|
@ -87,6 +89,9 @@ static int get_base_addr(enum s32cc_clk_source id, const struct s32cc_clk_drv *d
|
|||
case S32CC_ARM_PLL:
|
||||
*base = drv->armpll_base;
|
||||
break;
|
||||
case S32CC_ARM_DFS:
|
||||
*base = drv->armdfs_base;
|
||||
break;
|
||||
case S32CC_CGM1:
|
||||
*base = drv->cgm1_base;
|
||||
break;
|
||||
|
|
@ -551,6 +556,199 @@ static int enable_mux(const struct s32cc_clk_obj *module,
|
|||
return ret;
|
||||
}
|
||||
|
||||
static int enable_dfs(const struct s32cc_clk_obj *module,
|
||||
const struct s32cc_clk_drv *drv,
|
||||
unsigned int *depth)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
ret = update_stack_depth(depth);
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct s32cc_dfs *get_div_dfs(const struct s32cc_dfs_div *dfs_div)
|
||||
{
|
||||
const struct s32cc_clk_obj *parent = dfs_div->parent;
|
||||
|
||||
if (parent->type != s32cc_dfs_t) {
|
||||
ERROR("DFS DIV doesn't have a DFS as parent\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return s32cc_obj2dfs(parent);
|
||||
}
|
||||
|
||||
static struct s32cc_pll *dfsdiv2pll(const struct s32cc_dfs_div *dfs_div)
|
||||
{
|
||||
const struct s32cc_clk_obj *parent;
|
||||
const struct s32cc_dfs *dfs;
|
||||
|
||||
dfs = get_div_dfs(dfs_div);
|
||||
if (dfs == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
parent = dfs->parent;
|
||||
if (parent->type != s32cc_pll_t) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return s32cc_obj2pll(parent);
|
||||
}
|
||||
|
||||
static int get_dfs_mfi_mfn(unsigned long dfs_freq, const struct s32cc_dfs_div *dfs_div,
|
||||
uint32_t *mfi, uint32_t *mfn)
|
||||
{
|
||||
uint64_t factor64, tmp64, ofreq;
|
||||
uint32_t factor32;
|
||||
|
||||
unsigned long in = dfs_freq;
|
||||
unsigned long out = dfs_div->freq;
|
||||
|
||||
/**
|
||||
* factor = (IN / OUT) / 2
|
||||
* MFI = integer(factor)
|
||||
* MFN = (factor - MFI) * 36
|
||||
*/
|
||||
factor64 = ((((uint64_t)in) * FP_PRECISION) / ((uint64_t)out)) / 2ULL;
|
||||
tmp64 = factor64 / FP_PRECISION;
|
||||
if (tmp64 > UINT32_MAX) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
factor32 = (uint32_t)tmp64;
|
||||
*mfi = factor32;
|
||||
|
||||
tmp64 = ((factor64 - ((uint64_t)*mfi * FP_PRECISION)) * 36UL) / FP_PRECISION;
|
||||
if (tmp64 > UINT32_MAX) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
*mfn = (uint32_t)tmp64;
|
||||
|
||||
/* div_freq = in / (2 * (*mfi + *mfn / 36.0)) */
|
||||
factor64 = (((uint64_t)*mfn) * FP_PRECISION) / 36ULL;
|
||||
factor64 += ((uint64_t)*mfi) * FP_PRECISION;
|
||||
factor64 *= 2ULL;
|
||||
ofreq = (((uint64_t)in) * FP_PRECISION) / factor64;
|
||||
|
||||
if (ofreq != dfs_div->freq) {
|
||||
ERROR("Failed to find MFI and MFN settings for DFS DIV freq %lu\n",
|
||||
dfs_div->freq);
|
||||
ERROR("Nearest freq = %" PRIx64 "\n", ofreq);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int init_dfs_port(uintptr_t dfs_addr, uint32_t port,
|
||||
uint32_t mfi, uint32_t mfn)
|
||||
{
|
||||
uint32_t portsr, portolsr;
|
||||
uint32_t mask, old_mfi, old_mfn;
|
||||
uint32_t dvport;
|
||||
bool init_dfs;
|
||||
|
||||
dvport = mmio_read_32(DFS_DVPORTn(dfs_addr, port));
|
||||
|
||||
old_mfi = DFS_DVPORTn_MFI(dvport);
|
||||
old_mfn = DFS_DVPORTn_MFN(dvport);
|
||||
|
||||
portsr = mmio_read_32(DFS_PORTSR(dfs_addr));
|
||||
portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
|
||||
|
||||
/* Skip configuration if it's not needed */
|
||||
if (((portsr & BIT_32(port)) != 0U) &&
|
||||
((portolsr & BIT_32(port)) == 0U) &&
|
||||
(mfi == old_mfi) && (mfn == old_mfn)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
init_dfs = (portsr == 0U);
|
||||
|
||||
if (init_dfs) {
|
||||
mask = DFS_PORTRESET_MASK;
|
||||
} else {
|
||||
mask = DFS_PORTRESET_SET(BIT_32(port));
|
||||
}
|
||||
|
||||
mmio_write_32(DFS_PORTOLSR(dfs_addr), mask);
|
||||
mmio_write_32(DFS_PORTRESET(dfs_addr), mask);
|
||||
|
||||
while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & mask) != 0U) {
|
||||
}
|
||||
|
||||
if (init_dfs) {
|
||||
mmio_write_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
|
||||
}
|
||||
|
||||
mmio_write_32(DFS_DVPORTn(dfs_addr, port),
|
||||
DFS_DVPORTn_MFI_SET(mfi) | DFS_DVPORTn_MFN_SET(mfn));
|
||||
|
||||
if (init_dfs) {
|
||||
/* DFS clk enable programming */
|
||||
mmio_clrbits_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
|
||||
}
|
||||
|
||||
mmio_clrbits_32(DFS_PORTRESET(dfs_addr), BIT_32(port));
|
||||
|
||||
while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & BIT_32(port)) != BIT_32(port)) {
|
||||
}
|
||||
|
||||
portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
|
||||
if ((portolsr & DFS_PORTOLSR_LOL(port)) != 0U) {
|
||||
ERROR("Failed to lock DFS divider\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int enable_dfs_div(const struct s32cc_clk_obj *module,
|
||||
const struct s32cc_clk_drv *drv,
|
||||
unsigned int *depth)
|
||||
{
|
||||
const struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
|
||||
const struct s32cc_pll *pll;
|
||||
const struct s32cc_dfs *dfs;
|
||||
uintptr_t dfs_addr = 0UL;
|
||||
uint32_t mfi, mfn;
|
||||
int ret = 0;
|
||||
|
||||
ret = update_stack_depth(depth);
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
dfs = get_div_dfs(dfs_div);
|
||||
if (dfs == NULL) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
pll = dfsdiv2pll(dfs_div);
|
||||
if (pll == NULL) {
|
||||
ERROR("Failed to identify DFS divider's parent\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
ret = get_base_addr(dfs->instance, drv, &dfs_addr);
|
||||
if ((ret != 0) || (dfs_addr == 0UL)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
ret = get_dfs_mfi_mfn(pll->vco_freq, dfs_div, &mfi, &mfn);
|
||||
if (ret != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return init_dfs_port(dfs_addr, dfs_div->index, mfi, mfn);
|
||||
}
|
||||
|
||||
static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth)
|
||||
{
|
||||
const struct s32cc_clk_drv *drv = get_drv();
|
||||
|
|
@ -587,6 +785,12 @@ static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth
|
|||
case s32cc_fixed_div_t:
|
||||
ret = -ENOTSUP;
|
||||
break;
|
||||
case s32cc_dfs_t:
|
||||
ret = enable_dfs(module, drv, depth);
|
||||
break;
|
||||
case s32cc_dfs_div_t:
|
||||
ret = enable_dfs_div(module, drv, depth);
|
||||
break;
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
|
|
@ -793,6 +997,42 @@ static int set_mux_freq(const struct s32cc_clk_obj *module, unsigned long rate,
|
|||
return set_module_rate(&clk->desc, rate, orate, depth);
|
||||
}
|
||||
|
||||
static int set_dfs_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
|
||||
unsigned long *orate, unsigned int *depth)
|
||||
{
|
||||
struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
|
||||
const struct s32cc_dfs *dfs;
|
||||
int ret;
|
||||
|
||||
ret = update_stack_depth(depth);
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (dfs_div->parent == NULL) {
|
||||
ERROR("Failed to identify DFS divider's parent\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* Sanity check */
|
||||
dfs = s32cc_obj2dfs(dfs_div->parent);
|
||||
if (dfs->parent == NULL) {
|
||||
ERROR("Failed to identify DFS's parent\n");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if ((dfs_div->freq != 0U) && (dfs_div->freq != rate)) {
|
||||
ERROR("DFS DIV frequency was already set to %lu\n",
|
||||
dfs_div->freq);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
dfs_div->freq = rate;
|
||||
*orate = rate;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int set_module_rate(const struct s32cc_clk_obj *module,
|
||||
unsigned long rate, unsigned long *orate,
|
||||
unsigned int *depth)
|
||||
|
|
@ -804,6 +1044,8 @@ static int set_module_rate(const struct s32cc_clk_obj *module,
|
|||
return ret;
|
||||
}
|
||||
|
||||
ret = -EINVAL;
|
||||
|
||||
switch (module->type) {
|
||||
case s32cc_clk_t:
|
||||
ret = set_clk_freq(module, rate, orate, depth);
|
||||
|
|
@ -826,8 +1068,13 @@ static int set_module_rate(const struct s32cc_clk_obj *module,
|
|||
case s32cc_shared_clkmux_t:
|
||||
ret = set_mux_freq(module, rate, orate, depth);
|
||||
break;
|
||||
case s32cc_dfs_t:
|
||||
ERROR("Setting the frequency of a DFS is not allowed!");
|
||||
break;
|
||||
case s32cc_dfs_div_t:
|
||||
ret = set_dfs_div_freq(module, rate, orate, depth);
|
||||
break;
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue