[Patch RFC net-next 3/4] net: dsa: microchip: common ksz pvid get and set function

[Arun Ramadoss <arun.ramadoss@microchip.com>]

Add the helper function for getting and setting the pvid which will be
common for all ksz switches

Signed-off-by: Arun Ramadoss <arun.ramadoss@microchip.com>
---
 drivers/net/dsa/microchip/ksz8795.c    |  8 ++------
 drivers/net/dsa/microchip/ksz9477.c    |  6 +++---
 drivers/net/dsa/microchip/ksz_common.c | 21 +++++++++++++++++++++
 drivers/net/dsa/microchip/ksz_common.h | 11 +++++++++++
 4 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/drivers/net/dsa/microchip/ksz8795.c b/drivers/net/dsa/microchip/ksz8795.c
index 5dd73e994142..b8843697c5a5 100644
--- a/drivers/net/dsa/microchip/ksz8795.c
+++ b/drivers/net/dsa/microchip/ksz8795.c
@@ -1020,12 +1020,8 @@ int ksz8_port_vlan_add(struct ksz_device *dev, int port, u16 vlan_vid,
 		new_pvid = vlan_vid;
 
 	if (new_pvid) {
-		u16 vid;
 
-		ksz_pread16(dev, port, REG_PORT_CTRL_VID, &vid);
-		vid &= ~VLAN_VID_MASK;
-		vid |= new_pvid;
-		ksz_pwrite16(dev, port, REG_PORT_CTRL_VID, vid);
+		ksz_set_pvid(dev, port, new_pvid);
 
 		ksz8_port_enable_pvid(dev, port, true);
 	}
@@ -1042,7 +1038,7 @@ int ksz8_port_vlan_del(struct ksz_device *dev, int port,
 	if (ksz_is_ksz88x3(dev))
 		return -ENOTSUPP;
 
-	ksz_pread16(dev, port, REG_PORT_CTRL_VID, &pvid);
+	ksz_get_pvid(dev, port, &pvid);
 	pvid = pvid & 0xFFF;
 
 	ksz8_r_vlan_table(dev, vlan->vid, &data);
diff --git a/drivers/net/dsa/microchip/ksz9477.c b/drivers/net/dsa/microchip/ksz9477.c
index 81d24b89958b..a43a581520fb 100644
--- a/drivers/net/dsa/microchip/ksz9477.c
+++ b/drivers/net/dsa/microchip/ksz9477.c
@@ -396,7 +396,7 @@ int ksz9477_port_vlan_add(struct ksz_device *dev, int port, u16 vlan_vid,
 
 	/* change PVID */
 	if (flags & BRIDGE_VLAN_INFO_PVID)
-		ksz_pwrite16(dev, port, REG_PORT_DEFAULT_VID, vlan_vid);
+		ksz_set_pvid(dev, port, vlan_vid);
 
 	return 0;
 }
@@ -408,7 +408,7 @@ int ksz9477_port_vlan_del(struct ksz_device *dev, int port,
 	u32 vlan_table[3];
 	u16 pvid;
 
-	ksz_pread16(dev, port, REG_PORT_DEFAULT_VID, &pvid);
+	ksz_get_pvid(dev, port, &pvid);
 	pvid = pvid & 0xFFF;
 
 	if (ksz9477_get_vlan_table(dev, vlan->vid, vlan_table)) {
@@ -429,7 +429,7 @@ int ksz9477_port_vlan_del(struct ksz_device *dev, int port,
 		return -ETIMEDOUT;
 	}
 
-	ksz_pwrite16(dev, port, REG_PORT_DEFAULT_VID, pvid);
+	ksz_set_pvid(dev, port, pvid);
 
 	return 0;
 }
diff --git a/drivers/net/dsa/microchip/ksz_common.c b/drivers/net/dsa/microchip/ksz_common.c
index 97dbccb065a9..516fb9d35c87 100644
--- a/drivers/net/dsa/microchip/ksz_common.c
+++ b/drivers/net/dsa/microchip/ksz_common.c
@@ -14,6 +14,7 @@
 #include <linux/phy.h>
 #include <linux/etherdevice.h>
 #include <linux/if_bridge.h>
+#include <linux/if_vlan.h>
 #include <linux/of_device.h>
 #include <linux/of_net.h>
 #include <linux/micrel_phy.h>
@@ -258,6 +259,7 @@ static const u16 ksz8795_regs[] = {
 	[S_MULTICAST_CTRL]		= 0x04,
 	[P_XMII_CTRL_0]			= 0x06,
 	[P_XMII_CTRL_1]			= 0x56,
+	[P_DEFAULT_PVID]		= 0x03,
 };
 
 static const u32 ksz8795_masks[] = {
@@ -330,6 +332,7 @@ static const u16 ksz8863_regs[] = {
 	[S_START_CTRL]			= 0x01,
 	[S_BROADCAST_CTRL]		= 0x06,
 	[S_MULTICAST_CTRL]		= 0x04,
+	[P_DEFAULT_PVID]		= 0x03,
 };
 
 static const u32 ksz8863_masks[] = {
@@ -372,6 +375,7 @@ static const u16 ksz9477_regs[] = {
 	[S_MULTICAST_CTRL]		= 0x0331,
 	[P_XMII_CTRL_0]			= 0x0300,
 	[P_XMII_CTRL_1]			= 0x0301,
+	[P_DEFAULT_PVID]		= 0x0000,
 };
 
 static const u32 ksz9477_masks[] = {
@@ -1331,6 +1335,23 @@ static enum dsa_tag_protocol ksz_get_tag_protocol(struct dsa_switch *ds,
 	return proto;
 }
 
+void ksz_get_pvid(struct ksz_device *dev, int port, u16 *pvid)
+{
+	const u16 *regs = dev->info->regs;
+	u16 val;
+
+	ksz_pread16(dev, port, regs[P_DEFAULT_PVID], &val);
+
+	*pvid = val & VLAN_VID_MASK;
+}
+
+void ksz_set_pvid(struct ksz_device *dev, int port, u16 pvid)
+{
+	const u16 *regs = dev->info->regs;
+
+	ksz_prmw16(dev, port, regs[P_DEFAULT_PVID], VLAN_VID_MASK, pvid);
+}
+
 static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port,
 				   bool flag, struct netlink_ext_ack *extack)
 {
diff --git a/drivers/net/dsa/microchip/ksz_common.h b/drivers/net/dsa/microchip/ksz_common.h
index 9bb378b79a94..3bcd4e20bfaa 100644
--- a/drivers/net/dsa/microchip/ksz_common.h
+++ b/drivers/net/dsa/microchip/ksz_common.h
@@ -175,6 +175,7 @@ enum ksz_regs {
 	S_MULTICAST_CTRL,
 	P_XMII_CTRL_0,
 	P_XMII_CTRL_1,
+	P_DEFAULT_PVID,
 };
 
 enum ksz_masks {
@@ -319,6 +320,8 @@ void ksz_port_stp_state_set(struct dsa_switch *ds, int port, u8 state);
 bool ksz_get_gbit(struct ksz_device *dev, int port);
 phy_interface_t ksz_get_xmii(struct ksz_device *dev, int port, bool gbit);
 extern const struct ksz_chip_data ksz_switch_chips[];
+void ksz_get_pvid(struct ksz_device *dev, int port, u16 *pvid);
+void ksz_set_pvid(struct ksz_device *dev, int port, u16 pvid);
 
 /* Common register access functions */
 
@@ -432,6 +435,14 @@ static inline void ksz_prmw8(struct ksz_device *dev, int port, int offset,
 			   mask, val);
 }
 
+static inline void ksz_prmw16(struct ksz_device *dev, int port, int offset,
+			      u16 mask, u16 val)
+{
+	regmap_update_bits(dev->regmap[1],
+			   dev->dev_ops->get_port_addr(port, offset),
+			   mask, val);
+}
+
 static inline void ksz_regmap_lock(void *__mtx)
 {
 	struct mutex *mtx = __mtx;
-- 
2.36.1