[PATCH v2 3/3] net: mdio: mdio-thunder: support for clock-freq attribute

[Piyush Malgujar <pmalgujar@marvell.com>]

Added functionality to modify clock frequency via DTS entry.

Signed-off-by: Damian Eppel <deppel@marvell.com>
Signed-off-by: Piyush Malgujar <pmalgujar@marvell.com>
---
 drivers/net/mdio/mdio-cavium.h  |  1 +
 drivers/net/mdio/mdio-thunder.c | 63 +++++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/drivers/net/mdio/mdio-cavium.h b/drivers/net/mdio/mdio-cavium.h
index a2245d436f5dae4d6424b7c7bfca0aa969a3b3ad..ed4c48d8a38bd80e6a169f7a6d90c1f2a0daccfc 100644
--- a/drivers/net/mdio/mdio-cavium.h
+++ b/drivers/net/mdio/mdio-cavium.h
@@ -92,6 +92,7 @@ struct cavium_mdiobus {
 	struct mii_bus *mii_bus;
 	void __iomem *register_base;
 	enum cavium_mdiobus_mode mode;
+	u32 clk_freq;
 };
 
 #ifdef CONFIG_CAVIUM_OCTEON_SOC
diff --git a/drivers/net/mdio/mdio-thunder.c b/drivers/net/mdio/mdio-thunder.c
index 715c835ace785da345ac037177b0f291678e4c47..7ea6ef0a23f3f5d7df76e3a7aed007ed847f9140 100644
--- a/drivers/net/mdio/mdio-thunder.c
+++ b/drivers/net/mdio/mdio-thunder.c
@@ -14,11 +14,56 @@
 
 #include "mdio-cavium.h"
 
+#define PHASE_MIN 3
+#define PHASE_DFLT 16
+
 struct thunder_mdiobus_nexus {
 	void __iomem *bar0;
 	struct cavium_mdiobus *buses[4];
 };
 
+static inline u32 clk_freq(u32 phase)
+{
+	return (100000000U / (2 * (phase)));
+}
+
+static inline u32 calc_sample(u32 phase)
+{
+	return (2 * (phase) - 3);
+}
+
+static u32 _config_clk(u32 req_freq, u32 *phase, u32 *sample)
+{
+	unsigned int p;
+	u32 freq = 0, freq_prev;
+
+	for (p = PHASE_MIN; p < PHASE_DFLT; p++) {
+		freq_prev = freq;
+		freq = clk_freq(p);
+
+		if (req_freq >= freq)
+			break;
+	}
+
+	if (p == PHASE_DFLT)
+		freq = clk_freq(PHASE_DFLT);
+
+	if (p == PHASE_MIN || p == PHASE_DFLT)
+		goto out;
+
+	/* Check which clock value from the identified range
+	 * is closer to the requested value
+	 */
+	if ((freq_prev - req_freq) < (req_freq - freq)) {
+		p = p - 1;
+		freq = freq_prev;
+	}
+out:
+	*phase = p;
+	*sample = calc_sample(p);
+	return freq;
+}
+
 static int thunder_mdiobus_pci_probe(struct pci_dev *pdev,
 				     const struct pci_device_id *ent)
 {
@@ -56,6 +101,7 @@ static int thunder_mdiobus_pci_probe(struct pci_dev *pdev,
 	i = 0;
 	device_for_each_child_node(&pdev->dev, fwn) {
 		struct resource r;
+		u32 req_clk_freq;
 		struct mii_bus *mii_bus;
 		struct cavium_mdiobus *bus;
 		union cvmx_smix_clk smi_clk;
@@ -90,6 +136,23 @@ static int thunder_mdiobus_pci_probe(struct pci_dev *pdev,
 
 		smi_clk.u64 = oct_mdio_readq(bus->register_base + SMI_CLK);
 		smi_clk.s.clk_idle = 1;
+
+		if (!of_property_read_u32(node, "clock-frequency", &req_clk_freq)) {
+			u32 phase, sample;
+
+			dev_dbg(&pdev->dev, "requested bus clock frequency=%d\n",
+				req_clk_freq);
+
+			bus->clk_freq = _config_clk(req_clk_freq,
+						    &phase, &sample);
+
+			smi_clk.s.phase = phase;
+			smi_clk.s.sample_hi = (sample >> 4) & 0x1f;
+			smi_clk.s.sample = sample & 0xf;
+		} else {
+			bus->clk_freq = clk_freq(PHASE_DFLT);
+		}
+
 		oct_mdio_writeq(smi_clk.u64, bus->register_base + SMI_CLK);
 
 		smi_en.u64 = 0;
-- 
2.17.1