diff options
| -rw-r--r-- | drivers/spi/spi-pl022.c | 102 |
1 files changed, 51 insertions, 51 deletions
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c index 248674c8a8c2..4e1a1e3b2a8c 100644 --- a/drivers/spi/spi-pl022.c +++ b/drivers/spi/spi-pl022.c @@ -1790,67 +1790,67 @@ static int pl022_transfer(struct spi_device *spi, struct spi_message *msg) return 0; } -static int calculate_effective_freq(struct pl022 *pl022, - int freq, - struct ssp_clock_params *clk_freq) +static inline u32 spi_rate(u32 rate, u16 cpsdvsr, u16 scr) +{ + return rate / (cpsdvsr * (1 + scr)); +} + +static int calculate_effective_freq(struct pl022 *pl022, int freq, struct + ssp_clock_params * clk_freq) { /* Lets calculate the frequency parameters */ - u16 cpsdvsr = 2; - u16 scr = 0; - bool freq_found = false; - u32 rate; - u32 max_tclk; - u32 min_tclk; + u16 cpsdvsr = CPSDVR_MIN, scr = SCR_MIN; + u32 rate, max_tclk, min_tclk, best_freq = 0, best_cpsdvsr = 0, + best_scr = 0, tmp, found = 0; rate = clk_get_rate(pl022->clk); /* cpsdvscr = 2 & scr 0 */ - max_tclk = (rate / (CPSDVR_MIN * (1 + SCR_MIN))); + max_tclk = spi_rate(rate, CPSDVR_MIN, SCR_MIN); /* cpsdvsr = 254 & scr = 255 */ - min_tclk = (rate / (CPSDVR_MAX * (1 + SCR_MAX))); - - if ((freq <= max_tclk) && (freq >= min_tclk)) { - while (cpsdvsr <= CPSDVR_MAX && !freq_found) { - while (scr <= SCR_MAX && !freq_found) { - if ((rate / - (cpsdvsr * (1 + scr))) > freq) - scr += 1; - else { - /* - * This bool is made true when - * effective frequency >= - * target frequency is found - */ - freq_found = true; - if ((rate / - (cpsdvsr * (1 + scr))) != freq) { - if (scr == SCR_MIN) { - cpsdvsr -= 2; - scr = SCR_MAX; - } else - scr -= 1; - } - } - } - if (!freq_found) { - cpsdvsr += 2; - scr = SCR_MIN; - } - } - if (cpsdvsr != 0) { - dev_dbg(&pl022->adev->dev, - "SSP Effective Frequency is %u\n", - (rate / (cpsdvsr * (1 + scr)))); - clk_freq->cpsdvsr = (u8) (cpsdvsr & 0xFF); - clk_freq->scr = (u8) (scr & 0xFF); - dev_dbg(&pl022->adev->dev, - "SSP cpsdvsr = %d, scr = %d\n", - clk_freq->cpsdvsr, clk_freq->scr); - } - } else { + min_tclk = spi_rate(rate, CPSDVR_MAX, SCR_MAX); + + if (!((freq <= max_tclk) && (freq >= min_tclk))) { dev_err(&pl022->adev->dev, "controller data is incorrect: out of range frequency"); return -EINVAL; } + + /* + * best_freq will give closest possible available rate (<= requested + * freq) for all values of scr & cpsdvsr. + */ + while ((cpsdvsr <= CPSDVR_MAX) && !found) { + while (scr <= SCR_MAX) { + tmp = spi_rate(rate, cpsdvsr, scr); + + if (tmp > freq) + scr++; + /* + * If found exact value, update and break. + * If found more closer value, update and continue. + */ + else if ((tmp == freq) || (tmp > best_freq)) { + best_freq = tmp; + best_cpsdvsr = cpsdvsr; + best_scr = scr; + + if (tmp == freq) + break; + } + scr++; + } + cpsdvsr += 2; + scr = SCR_MIN; + } + + clk_freq->cpsdvsr = (u8) (best_cpsdvsr & 0xFF); + clk_freq->scr = (u8) (best_scr & 0xFF); + dev_dbg(&pl022->adev->dev, + "SSP Target Frequency is: %u, Effective Frequency is %u\n", + freq, best_freq); + dev_dbg(&pl022->adev->dev, "SSP cpsdvsr = %d, scr = %d\n", + clk_freq->cpsdvsr, clk_freq->scr); + return 0; } |
