[PATCH AUTOSEL for 4.9 122/190] soc/fsl/qe: round brg_freq to 1kHz granularity

[Sasha Levin]

From: Valentin Longchamp <valentin.longchamp@xxxxxxxxxxx>

[ Upstream commit 2ccf80b7566cc035d903dd0ac5d7ebd25c2c1060 ]

Because of integer computation rounding in u-boot (that sets the QE
brg-frequency DTS prop), the clk value is 99999999 Hz even though it is
100 MHz.

When setting brg clks that are exact divisors of 100 MHz, this small
differnce plays a role and can result in lower clks to be output (for
instance 20 MHz - divide by 5 - results in 16.666 MHz - divide by 6).

This patch fixes that by "forcing" the brg_clk to the nearest kHz when
the difference is below 2 integer rounding errors (i.e. 4).

Signed-off-by: Valentin Longchamp <valentin.longchamp@xxxxxxxxxxx>
Signed-off-by: Scott Wood <oss@xxxxxxxxxxxx>
Signed-off-by: Sasha Levin <alexander.levin@xxxxxxxxxxxxx>
---
 drivers/soc/fsl/qe/qe.c | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/drivers/soc/fsl/qe/qe.c b/drivers/soc/fsl/qe/qe.c
index 2707a827261b..5482302d3d9e 100644
--- a/drivers/soc/fsl/qe/qe.c
+++ b/drivers/soc/fsl/qe/qe.c
@@ -163,11 +163,15 @@ EXPORT_SYMBOL(qe_issue_cmd);
  */
 static unsigned int brg_clk = 0;
 
+#define CLK_GRAN	(1000)
+#define CLK_GRAN_LIMIT	(5)
+
 unsigned int qe_get_brg_clk(void)
 {
 	struct device_node *qe;
 	int size;
 	const u32 *prop;
+	unsigned int mod;
 
 	if (brg_clk)
 		return brg_clk;
@@ -185,6 +189,15 @@ unsigned int qe_get_brg_clk(void)
 
 	of_node_put(qe);
 
+	/* round this if near to a multiple of CLK_GRAN */
+	mod = brg_clk % CLK_GRAN;
+	if (mod) {
+		if (mod < CLK_GRAN_LIMIT)
+			brg_clk -= mod;
+		else if (mod > (CLK_GRAN - CLK_GRAN_LIMIT))
+			brg_clk += CLK_GRAN - mod;
+	}
+
 	return brg_clk;
 }
 EXPORT_SYMBOL(qe_get_brg_clk);
-- 
2.14.1