[PATCH net] net: dsa: microchip: fix KSZ9477 set_ageing_time function

[<Tristram.Ha@microchip.com>]

From: Tristram Ha <tristram.ha@microchip.com>

The aging count is not a simple 11-bit value but comprises a 3-bit
multiplier and an 8-bit second count.  The code tries to find a set of
values with result close to the specifying value.

Note LAN937X has similar operation but provides an option to use
millisecond instead of second so there will be a separate fix in the
future.

Fixes: 2c119d9982b1 ("net: dsa: microchip: add the support for set_ageing_time")
Signed-off-by: Tristram Ha <tristram.ha@microchip.com>
---
 drivers/net/dsa/microchip/ksz9477.c     | 64 +++++++++++++++++++++----
 drivers/net/dsa/microchip/ksz9477_reg.h |  1 -
 2 files changed, 54 insertions(+), 11 deletions(-)

diff --git a/drivers/net/dsa/microchip/ksz9477.c b/drivers/net/dsa/microchip/ksz9477.c
index f8ad7833f5d9..1af11aee3119 100644
--- a/drivers/net/dsa/microchip/ksz9477.c
+++ b/drivers/net/dsa/microchip/ksz9477.c
@@ -1099,26 +1099,70 @@ void ksz9477_get_caps(struct ksz_device *dev, int port,
 int ksz9477_set_ageing_time(struct ksz_device *dev, unsigned int msecs)
 {
 	u32 secs = msecs / 1000;
+	u8 first, last, mult, i;
+	int min, ret;
+	int diff[8];
 	u8 value;
 	u8 data;
-	int ret;
 
-	value = FIELD_GET(SW_AGE_PERIOD_7_0_M, secs);
+	/* The aging timer comprises a 3-bit multiplier and an 8-bit second
+	 * value.  Either of them cannot be zero.  The maximum timer is then
+	 * 7 * 255 = 1785.
+	 */
+	if (!secs)
+		secs = 1;
 
-	ret = ksz_write8(dev, REG_SW_LUE_CTRL_3, value);
+	ret = ksz_read8(dev, REG_SW_LUE_CTRL_0, &value);
 	if (ret < 0)
 		return ret;
 
-	data = FIELD_GET(SW_AGE_PERIOD_10_8_M, secs);
+	/* Check whether there is need to update the multiplier. */
+	mult = FIELD_GET(SW_AGE_CNT_M, value);
+	if (mult > 0) {
+		/* Try to use the same multiplier already in the register. */
+		min = secs / mult;
+		if (min <= 0xff && min * mult == secs)
+			return ksz_write8(dev, REG_SW_LUE_CTRL_3, min);
+	}
 
-	ret = ksz_read8(dev, REG_SW_LUE_CTRL_0, &value);
-	if (ret < 0)
-		return ret;
+	/* Return error if too large. */
+	if (secs > 7 * 0xff)
+		return -EINVAL;
+
+	/* Find out which combination of multiplier * value results in a timer
+	 * value close to the specified timer value.
+	 */
+	first = (secs + 0xfe) / 0xff;
+	for (i = first; i <= 7; i++) {
+		min = secs / i;
+		diff[i] = secs - i * min;
+		if (!diff[i]) {
+			i++;
+			break;
+		}
+	}
+
+	last = i;
+	min = 0xff;
+	for (i = last - 1; i >= first; i--) {
+		if (diff[i] < min) {
+			data = i;
+			min = diff[i];
+		}
+		if (!min)
+			break;
+	}
 
-	value &= ~SW_AGE_CNT_M;
-	value |= FIELD_PREP(SW_AGE_CNT_M, data);
+	if (mult != data) {
+		value &= ~SW_AGE_CNT_M;
+		value |= FIELD_PREP(SW_AGE_CNT_M, data);
+		ret = ksz_write8(dev, REG_SW_LUE_CTRL_0, value);
+		if (ret)
+			return ret;
+	}
 
-	return ksz_write8(dev, REG_SW_LUE_CTRL_0, value);
+	value = secs / data;
+	return ksz_write8(dev, REG_SW_LUE_CTRL_3, value);
 }
 
 void ksz9477_port_queue_split(struct ksz_device *dev, int port)
diff --git a/drivers/net/dsa/microchip/ksz9477_reg.h b/drivers/net/dsa/microchip/ksz9477_reg.h
index f3a205ee483f..0c55a540f20d 100644
--- a/drivers/net/dsa/microchip/ksz9477_reg.h
+++ b/drivers/net/dsa/microchip/ksz9477_reg.h
@@ -171,7 +171,6 @@
 #define SW_DROP_INVALID_VID		BIT(6)
 #define SW_AGE_CNT_M			GENMASK(5, 3)
 #define SW_AGE_CNT_S			3
-#define SW_AGE_PERIOD_10_8_M		GENMASK(10, 8)
 #define SW_RESV_MCAST_ENABLE		BIT(2)
 #define SW_HASH_OPTION_M		0x03
 #define SW_HASH_OPTION_CRC		1
-- 
2.34.1