Orange Pi5 kernel

^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   1) // SPDX-License-Identifier: GPL-2.0-only
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   3)  * Marvell PXA family clocks
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   4)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   5)  * Copyright (C) 2014 Robert Jarzmik
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   6)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   7)  * Common clock code for PXA clocks ("CKEN" type clocks + DT)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   8)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   9) #include <linux/clk.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  10) #include <linux/clk-provider.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  11) #include <linux/clkdev.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  12) #include <linux/io.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  13) #include <linux/of.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  14) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  15) #include <dt-bindings/clock/pxa-clock.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  16) #include "clk-pxa.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  17) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  18) #define KHz 1000
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  19) #define MHz (1000 * 1000)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  20) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  21) #define MDREFR_K0DB4	(1 << 29)	/* SDCLK0 Divide by 4 Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  22) #define MDREFR_K2FREE	(1 << 25)	/* SDRAM Free-Running Control */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  23) #define MDREFR_K1FREE	(1 << 24)	/* SDRAM Free-Running Control */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  24) #define MDREFR_K0FREE	(1 << 23)	/* SDRAM Free-Running Control */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  25) #define MDREFR_SLFRSH	(1 << 22)	/* SDRAM Self-Refresh Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  26) #define MDREFR_APD	(1 << 20)	/* SDRAM/SSRAM Auto-Power-Down Enable */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  27) #define MDREFR_K2DB2	(1 << 19)	/* SDCLK2 Divide by 2 Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  28) #define MDREFR_K2RUN	(1 << 18)	/* SDCLK2 Run Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  29) #define MDREFR_K1DB2	(1 << 17)	/* SDCLK1 Divide by 2 Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  30) #define MDREFR_K1RUN	(1 << 16)	/* SDCLK1 Run Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  31) #define MDREFR_E1PIN	(1 << 15)	/* SDCKE1 Level Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  32) #define MDREFR_K0DB2	(1 << 14)	/* SDCLK0 Divide by 2 Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  33) #define MDREFR_K0RUN	(1 << 13)	/* SDCLK0 Run Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  34) #define MDREFR_E0PIN	(1 << 12)	/* SDCKE0 Level Control/Status */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  35) #define MDREFR_DB2_MASK	(MDREFR_K2DB2 | MDREFR_K1DB2)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  36) #define MDREFR_DRI_MASK	0xFFF
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  37) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  38) static DEFINE_SPINLOCK(pxa_clk_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  39) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  40) static struct clk *pxa_clocks[CLK_MAX];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  41) static struct clk_onecell_data onecell_data = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  42) 	.clks = pxa_clocks,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  43) 	.clk_num = CLK_MAX,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  44) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  45) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  46) struct pxa_clk {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  47) 	struct clk_hw hw;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  48) 	struct clk_fixed_factor lp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  49) 	struct clk_fixed_factor hp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  50) 	struct clk_gate gate;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  51) 	bool (*is_in_low_power)(void);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  52) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  53) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  54) #define to_pxa_clk(_hw) container_of(_hw, struct pxa_clk, hw)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  55) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  56) static unsigned long cken_recalc_rate(struct clk_hw *hw,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  57) 				      unsigned long parent_rate)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  58) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  59) 	struct pxa_clk *pclk = to_pxa_clk(hw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  60) 	struct clk_fixed_factor *fix;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  61) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  62) 	if (!pclk->is_in_low_power || pclk->is_in_low_power())
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  63) 		fix = &pclk->lp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  64) 	else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  65) 		fix = &pclk->hp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  66) 	__clk_hw_set_clk(&fix->hw, hw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  67) 	return clk_fixed_factor_ops.recalc_rate(&fix->hw, parent_rate);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  68) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  69) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  70) static const struct clk_ops cken_rate_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  71) 	.recalc_rate = cken_recalc_rate,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  72) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  73) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  74) static u8 cken_get_parent(struct clk_hw *hw)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  75) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  76) 	struct pxa_clk *pclk = to_pxa_clk(hw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  77) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  78) 	if (!pclk->is_in_low_power)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  79) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  80) 	return pclk->is_in_low_power() ? 0 : 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  81) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  82) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  83) static const struct clk_ops cken_mux_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  84) 	.get_parent = cken_get_parent,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  85) 	.set_parent = dummy_clk_set_parent,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  86) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  87) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  88) void __init clkdev_pxa_register(int ckid, const char *con_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  89) 				const char *dev_id, struct clk *clk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  90) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  91) 	if (!IS_ERR(clk) && (ckid != CLK_NONE))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  92) 		pxa_clocks[ckid] = clk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  93) 	if (!IS_ERR(clk))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  94) 		clk_register_clkdev(clk, con_id, dev_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  95) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  96) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  97) int __init clk_pxa_cken_init(const struct desc_clk_cken *clks, int nb_clks)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  98) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  99) 	int i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) 	struct pxa_clk *pxa_clk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) 	struct clk *clk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) 	for (i = 0; i < nb_clks; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) 		pxa_clk = kzalloc(sizeof(*pxa_clk), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) 		pxa_clk->is_in_low_power = clks[i].is_in_low_power;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) 		pxa_clk->lp = clks[i].lp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) 		pxa_clk->hp = clks[i].hp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) 		pxa_clk->gate = clks[i].gate;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) 		pxa_clk->gate.lock = &pxa_clk_lock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) 		clk = clk_register_composite(NULL, clks[i].name,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) 					     clks[i].parent_names, 2,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) 					     &pxa_clk->hw, &cken_mux_ops,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) 					     &pxa_clk->hw, &cken_rate_ops,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) 					     &pxa_clk->gate.hw, &clk_gate_ops,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) 					     clks[i].flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) 		clkdev_pxa_register(clks[i].ckid, clks[i].con_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) 				    clks[i].dev_id, clk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) void __init clk_pxa_dt_common_init(struct device_node *np)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) 	of_clk_add_provider(np, of_clk_src_onecell_get, &onecell_data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) void pxa2xx_core_turbo_switch(bool on)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) 	unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) 	unsigned int unused, clkcfg;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) 	local_irq_save(flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) 	asm("mrc p14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) 	clkcfg &= ~CLKCFG_TURBO & ~CLKCFG_HALFTURBO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) 	if (on)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) 		clkcfg |= CLKCFG_TURBO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) 	clkcfg |= CLKCFG_FCS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) 	asm volatile(
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) 	"	b	2f\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) 	"	.align	5\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) 	"1:	mcr	p14, 0, %1, c6, c0, 0\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) 	"	b	3f\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) 	"2:	b	1b\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) 	"3:	nop\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) 		: "=&r" (unused) : "r" (clkcfg));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) 	local_irq_restore(flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) void pxa2xx_cpll_change(struct pxa2xx_freq *freq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) 			u32 (*mdrefr_dri)(unsigned int), void __iomem *mdrefr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) 			void __iomem *cccr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) 	unsigned int clkcfg = freq->clkcfg;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) 	unsigned int unused, preset_mdrefr, postset_mdrefr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) 	unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) 	local_irq_save(flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) 	/* Calculate the next MDREFR.  If we're slowing down the SDRAM clock
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) 	 * we need to preset the smaller DRI before the change.	 If we're
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) 	 * speeding up we need to set the larger DRI value after the change.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) 	preset_mdrefr = postset_mdrefr = readl(mdrefr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) 	if ((preset_mdrefr & MDREFR_DRI_MASK) > mdrefr_dri(freq->membus_khz)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) 		preset_mdrefr = (preset_mdrefr & ~MDREFR_DRI_MASK);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) 		preset_mdrefr |= mdrefr_dri(freq->membus_khz);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) 	postset_mdrefr =
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) 		(postset_mdrefr & ~MDREFR_DRI_MASK) |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) 		mdrefr_dri(freq->membus_khz);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) 	/* If we're dividing the memory clock by two for the SDRAM clock, this
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) 	 * must be set prior to the change.  Clearing the divide must be done
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) 	 * after the change.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) 	if (freq->div2) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) 		preset_mdrefr  |= MDREFR_DB2_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) 		postset_mdrefr |= MDREFR_DB2_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) 	} else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) 		postset_mdrefr &= ~MDREFR_DB2_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) 	/* Set new the CCCR and prepare CLKCFG */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) 	writel(freq->cccr, cccr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) 	asm volatile(
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) 	"	ldr	r4, [%1]\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) 	"	b	2f\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) 	"	.align	5\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) 	"1:	str	%3, [%1]		/* preset the MDREFR */\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) 	"	mcr	p14, 0, %2, c6, c0, 0	/* set CLKCFG[FCS] */\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) 	"	str	%4, [%1]		/* postset the MDREFR */\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) 	"	b	3f\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) 	"2:	b	1b\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) 	"3:	nop\n"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) 	     : "=&r" (unused)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) 	     : "r" (mdrefr), "r" (clkcfg), "r" (preset_mdrefr),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) 	       "r" (postset_mdrefr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) 	     : "r4", "r5");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) 	local_irq_restore(flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) int pxa2xx_determine_rate(struct clk_rate_request *req,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) 			  struct pxa2xx_freq *freqs, int nb_freqs)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) 	int i, closest_below = -1, closest_above = -1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) 	unsigned long rate;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) 	for (i = 0; i < nb_freqs; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) 		rate = freqs[i].cpll;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) 		if (rate == req->rate)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) 			break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) 		if (rate < req->min_rate)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) 			continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) 		if (rate > req->max_rate)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) 			continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) 		if (rate <= req->rate)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) 			closest_below = i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) 		if ((rate >= req->rate) && (closest_above == -1))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) 			closest_above = i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) 	req->best_parent_hw = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) 	if (i < nb_freqs) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) 		rate = req->rate;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) 	} else if (closest_below >= 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) 		rate = freqs[closest_below].cpll;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) 	} else if (closest_above >= 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) 		rate = freqs[closest_above].cpll;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) 	} else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) 		pr_debug("%s(rate=%lu) no match\n", __func__, req->rate);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) 	pr_debug("%s(rate=%lu) rate=%lu\n", __func__, req->rate, rate);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) 	req->rate = rate;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) }