path: root/target/linux/ramips/patches-4.3/0512-net-mediatek-add-swconfig-driver-for-esw_rt3050.patch

From 4473f30809eed09037e1932a0c1805172cd997f7 Mon Sep 17 00:00:00 2001
From: John Crispin <blogic@openwrt.org>
Date: Mon, 14 Dec 2015 22:07:31 +0100
Subject: [PATCH 512/513] net: mediatek: add swconfig driver for esw_rt3050

Signed-off-by: John Crispin <blogic@openwrt.org>
---
 drivers/net/ethernet/mediatek/esw_rt3050.c |  805 ++++++++++++++++++++++++++++
 1 file changed, 805 insertions(+)

diff --git a/drivers/net/ethernet/mediatek/esw_rt3050.c b/drivers/net/ethernet/mediatek/esw_rt3050.c
index 670ae16..352d50a 100644
--- a/drivers/net/ethernet/mediatek/esw_rt3050.c
+++ b/drivers/net/ethernet/mediatek/esw_rt3050.c
@@ -17,6 +17,8 @@
 #include <linux/platform_device.h>
 #include <asm/mach-ralink/ralink_regs.h>
 
+#include <linux/switch.h>
+
 #include "mtk_eth_soc.h"
 
 /* HW limitations for this switch:
@@ -141,6 +143,8 @@
 #define RT305X_ESW_PORT5		5
 #define RT305X_ESW_PORT6		6
 
+#define RT305X_ESW_PORTS_NONE		0
+
 #define RT305X_ESW_PMAP_LLLLLL		0x3f
 #define RT305X_ESW_PMAP_LLLLWL		0x2f
 #define RT305X_ESW_PMAP_WLLLLL		0x3e
@@ -158,15 +162,51 @@
 #define RT305X_ESW_PORTS_ALL						\
 		(RT305X_ESW_PORTS_NOCPU | RT305X_ESW_PORTS_CPU)
 
+#define RT305X_ESW_NUM_VLANS		16
+#define RT305X_ESW_NUM_VIDS		4096
 #define RT305X_ESW_NUM_PORTS		7
+#define RT305X_ESW_NUM_LANWAN		6
 #define RT305X_ESW_NUM_LEDS		5
 
+#define RT5350_ESW_REG_PXTPC(_x)	(0x150 + (4 * _x))
 #define RT5350_EWS_REG_LED_POLARITY	0x168
 #define RT5350_RESET_EPHY		BIT(24)
 
+enum {
+	/* Global attributes. */
+	RT305X_ESW_ATTR_ENABLE_VLAN,
+	RT305X_ESW_ATTR_ALT_VLAN_DISABLE,
+	RT305X_ESW_ATTR_BC_STATUS,
+	RT305X_ESW_ATTR_LED_FREQ,
+	/* Port attributes. */
+	RT305X_ESW_ATTR_PORT_DISABLE,
+	RT305X_ESW_ATTR_PORT_DOUBLETAG,
+	RT305X_ESW_ATTR_PORT_UNTAG,
+	RT305X_ESW_ATTR_PORT_LED,
+	RT305X_ESW_ATTR_PORT_LAN,
+	RT305X_ESW_ATTR_PORT_RECV_BAD,
+	RT305X_ESW_ATTR_PORT_RECV_GOOD,
+	RT5350_ESW_ATTR_PORT_TR_BAD,
+	RT5350_ESW_ATTR_PORT_TR_GOOD,
+};
+
 struct esw_port {
 	bool	disable;
+	bool	doubletag;
+	bool	untag;
 	u8	led;
+	u16	pvid;
+};
+
+struct esw_vlan {
+	u8	ports;
+	u16	vid;
+};
+
+enum {
+	RT305X_ESW_VLAN_CONFIG_NONE = 0,
+	RT305X_ESW_VLAN_CONFIG_LLLLW,
+	RT305X_ESW_VLAN_CONFIG_WLLLL,
 };
 
 struct rt305x_esw {
@@ -180,6 +220,12 @@ struct rt305x_esw {
 	unsigned char		port_map;
 	unsigned int		reg_led_polarity;
 
+	struct switch_dev	swdev;
+	bool			global_vlan_enable;
+	bool			alt_vlan_disable;
+	int			bc_storm_protect;
+	int			led_frequency;
+	struct esw_vlan vlans[RT305X_ESW_NUM_VLANS];
 	struct esw_port ports[RT305X_ESW_NUM_PORTS];
 
 };
@@ -252,6 +298,71 @@ out:
 	return ret;
 }
 
+static unsigned esw_get_vlan_id(struct rt305x_esw *esw, unsigned vlan)
+{
+	unsigned s;
+	unsigned val;
+
+	s = RT305X_ESW_VLANI_VID_S * (vlan % 2);
+	val = esw_r32(esw, RT305X_ESW_REG_VLANI(vlan / 2));
+	val = (val >> s) & RT305X_ESW_VLANI_VID_M;
+
+	return val;
+}
+
+static void esw_set_vlan_id(struct rt305x_esw *esw, unsigned vlan, unsigned vid)
+{
+	unsigned s;
+
+	s = RT305X_ESW_VLANI_VID_S * (vlan % 2);
+	esw_rmw(esw,
+		       RT305X_ESW_REG_VLANI(vlan / 2),
+		       RT305X_ESW_VLANI_VID_M << s,
+		       (vid & RT305X_ESW_VLANI_VID_M) << s);
+}
+
+static unsigned esw_get_pvid(struct rt305x_esw *esw, unsigned port)
+{
+	unsigned s, val;
+
+	s = RT305X_ESW_PVIDC_PVID_S * (port % 2);
+	val = esw_r32(esw, RT305X_ESW_REG_PVIDC(port / 2));
+	return (val >> s) & RT305X_ESW_PVIDC_PVID_M;
+}
+
+static void esw_set_pvid(struct rt305x_esw *esw, unsigned port, unsigned pvid)
+{
+	unsigned s;
+
+	s = RT305X_ESW_PVIDC_PVID_S * (port % 2);
+	esw_rmw(esw,
+		       RT305X_ESW_REG_PVIDC(port / 2),
+		       RT305X_ESW_PVIDC_PVID_M << s,
+		       (pvid & RT305X_ESW_PVIDC_PVID_M) << s);
+}
+
+static unsigned esw_get_vmsc(struct rt305x_esw *esw, unsigned vlan)
+{
+	unsigned s, val;
+
+	s = RT305X_ESW_VMSC_MSC_S * (vlan % 4);
+	val = esw_r32(esw, RT305X_ESW_REG_VMSC(vlan / 4));
+	val = (val >> s) & RT305X_ESW_VMSC_MSC_M;
+
+	return val;
+}
+
+static void esw_set_vmsc(struct rt305x_esw *esw, unsigned vlan, unsigned msc)
+{
+	unsigned s;
+
+	s = RT305X_ESW_VMSC_MSC_S * (vlan % 4);
+	esw_rmw(esw,
+		       RT305X_ESW_REG_VMSC(vlan / 4),
+		       RT305X_ESW_VMSC_MSC_M << s,
+		       (msc & RT305X_ESW_VMSC_MSC_M) << s);
+}
+
 static unsigned esw_get_port_disable(struct rt305x_esw *esw)
 {
 	unsigned reg;
@@ -261,6 +372,59 @@ static unsigned esw_get_port_disable(struct rt305x_esw *esw)
 	       RT305X_ESW_POC0_DIS_PORT_M;
 }
 
+static void esw_set_port_disable(struct rt305x_esw *esw, unsigned disable_mask)
+{
+	unsigned old_mask;
+	unsigned enable_mask;
+	unsigned changed;
+	int i;
+
+	old_mask = esw_get_port_disable(esw);
+	changed = old_mask ^ disable_mask;
+	enable_mask = old_mask & disable_mask;
+
+	/* enable before writing to MII */
+	esw_rmw(esw, RT305X_ESW_REG_POC0,
+		       (RT305X_ESW_POC0_DIS_PORT_M <<
+			RT305X_ESW_POC0_DIS_PORT_S),
+		       enable_mask << RT305X_ESW_POC0_DIS_PORT_S);
+
+	for (i = 0; i < RT305X_ESW_NUM_LEDS; i++) {
+		if (!(changed & (1 << i)))
+			continue;
+		if (disable_mask & (1 << i)) {
+			/* disable */
+			rt305x_mii_write(esw, i, MII_BMCR,
+					 BMCR_PDOWN);
+		} else {
+			/* enable */
+			rt305x_mii_write(esw, i, MII_BMCR,
+					 BMCR_FULLDPLX |
+					 BMCR_ANENABLE |
+					 BMCR_ANRESTART |
+					 BMCR_SPEED100);
+		}
+	}
+
+	/* disable after writing to MII */
+	esw_rmw(esw, RT305X_ESW_REG_POC0,
+		       (RT305X_ESW_POC0_DIS_PORT_M <<
+			RT305X_ESW_POC0_DIS_PORT_S),
+		       disable_mask << RT305X_ESW_POC0_DIS_PORT_S);
+}
+
+static void esw_set_gsc(struct rt305x_esw *esw)
+{
+	esw_rmw(esw, RT305X_ESW_REG_SGC,
+		RT305X_ESW_GSC_BC_STROM_MASK << RT305X_ESW_GSC_BC_STROM_SHIFT,
+		esw->bc_storm_protect << RT305X_ESW_GSC_BC_STROM_SHIFT);
+	esw_rmw(esw, RT305X_ESW_REG_SGC,
+		RT305X_ESW_GSC_LED_FREQ_MASK << RT305X_ESW_GSC_LED_FREQ_SHIFT,
+		esw->led_frequency << RT305X_ESW_GSC_LED_FREQ_SHIFT);
+}
+
+static int esw_apply_config(struct switch_dev *dev);
+
 static void esw_hw_init(struct rt305x_esw *esw)
 {
 	int i;
@@ -519,6 +683,9 @@ static void esw_hw_init(struct rt305x_esw *esw)
 	for (i = 0; i < RT305X_ESW_NUM_LEDS; i++)
 		esw->ports[i].led = 0x05;
 
+	/* Apply the empty config. */
+	esw_apply_config(&esw->swdev);
+
 	/* Only unmask the port change interrupt */
 	esw_w32(esw, ~RT305X_ESW_PORT_ST_CHG, RT305X_ESW_REG_IMR);
 }
@@ -541,11 +708,629 @@ static irqreturn_t esw_interrupt(int irq, void *_esw)
 	return IRQ_HANDLED;
 }
 
+static int esw_apply_config(struct switch_dev *dev)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int i;
+	u8 disable = 0;
+	u8 doubletag = 0;
+	u8 en_vlan = 0;
+	u8 untag = 0;
+
+	for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
+		u32 vid, vmsc;
+		if (esw->global_vlan_enable) {
+			vid = esw->vlans[i].vid;
+			vmsc = esw->vlans[i].ports;
+		} else {
+			vid = RT305X_ESW_VLAN_NONE;
+			vmsc = RT305X_ESW_PORTS_NONE;
+		}
+		esw_set_vlan_id(esw, i, vid);
+		esw_set_vmsc(esw, i, vmsc);
+	}
+
+	for (i = 0; i < RT305X_ESW_NUM_PORTS; i++) {
+		u32 pvid;
+		disable |= esw->ports[i].disable << i;
+		if (esw->global_vlan_enable) {
+			doubletag |= esw->ports[i].doubletag << i;
+			en_vlan   |= 1                       << i;
+			untag     |= esw->ports[i].untag     << i;
+			pvid       = esw->ports[i].pvid;
+		} else {
+			int x = esw->alt_vlan_disable ? 0 : 1;
+			doubletag |= x << i;
+			en_vlan   |= x << i;
+			untag     |= x << i;
+			pvid       = 0;
+		}
+		esw_set_pvid(esw, i, pvid);
+		if (i < RT305X_ESW_NUM_LEDS)
+			esw_w32(esw, esw->ports[i].led,
+				      RT305X_ESW_REG_P0LED + 4*i);
+	}
+
+	esw_set_gsc(esw);
+	esw_set_port_disable(esw, disable);
+	esw_rmw(esw, RT305X_ESW_REG_SGC2,
+		       (RT305X_ESW_SGC2_DOUBLE_TAG_M <<
+			RT305X_ESW_SGC2_DOUBLE_TAG_S),
+		       doubletag << RT305X_ESW_SGC2_DOUBLE_TAG_S);
+	esw_rmw(esw, RT305X_ESW_REG_PFC1,
+		       RT305X_ESW_PFC1_EN_VLAN_M << RT305X_ESW_PFC1_EN_VLAN_S,
+		       en_vlan << RT305X_ESW_PFC1_EN_VLAN_S);
+	esw_rmw(esw, RT305X_ESW_REG_POC2,
+		       RT305X_ESW_POC2_UNTAG_EN_M << RT305X_ESW_POC2_UNTAG_EN_S,
+		       untag << RT305X_ESW_POC2_UNTAG_EN_S);
+
+	if (!esw->global_vlan_enable) {
+		/*
+		 * Still need to put all ports into vlan 0 or they'll be
+		 * isolated.
+		 * NOTE: vlan 0 is special, no vlan tag is prepended
+		 */
+		esw_set_vlan_id(esw, 0, 0);
+		esw_set_vmsc(esw, 0, RT305X_ESW_PORTS_ALL);
+	}
+
+	return 0;
+}
+
+static int esw_reset_switch(struct switch_dev *dev)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	esw->global_vlan_enable = 0;
+	memset(esw->ports, 0, sizeof(esw->ports));
+	memset(esw->vlans, 0, sizeof(esw->vlans));
+	esw_hw_init(esw);
+
+	return 0;
+}
+
+static int esw_get_vlan_enable(struct switch_dev *dev,
+			   const struct switch_attr *attr,
+			   struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	val->value.i = esw->global_vlan_enable;
+
+	return 0;
+}
+
+static int esw_set_vlan_enable(struct switch_dev *dev,
+			   const struct switch_attr *attr,
+			   struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	esw->global_vlan_enable = val->value.i != 0;
+
+	return 0;
+}
+
+static int esw_get_alt_vlan_disable(struct switch_dev *dev,
+				const struct switch_attr *attr,
+				struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	val->value.i = esw->alt_vlan_disable;
+
+	return 0;
+}
+
+static int esw_set_alt_vlan_disable(struct switch_dev *dev,
+				const struct switch_attr *attr,
+				struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	esw->alt_vlan_disable = val->value.i != 0;
+
+	return 0;
+}
+
+static int
+rt305x_esw_set_bc_status(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	esw->bc_storm_protect = val->value.i & RT305X_ESW_GSC_BC_STROM_MASK;
+
+	return 0;
+}
+
+static int
+rt305x_esw_get_bc_status(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	val->value.i = esw->bc_storm_protect;
+
+	return 0;
+}
+
+static int
+rt305x_esw_set_led_freq(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	esw->led_frequency = val->value.i & RT305X_ESW_GSC_LED_FREQ_MASK;
+
+	return 0;
+}
+
+static int
+rt305x_esw_get_led_freq(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	val->value.i = esw->led_frequency;
+
+	return 0;
+}
+
+static int esw_get_port_link(struct switch_dev *dev,
+			 int port,
+			 struct switch_port_link *link)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	u32 speed, poa;
+
+	if (port < 0 || port >= RT305X_ESW_NUM_PORTS)
+		return -EINVAL;
+
+	poa = esw_r32(esw, RT305X_ESW_REG_POA) >> port;
+
+	link->link = (poa >> RT305X_ESW_LINK_S) & 1;
+	link->duplex = (poa >> RT305X_ESW_DUPLEX_S) & 1;
+	if (port < RT305X_ESW_NUM_LEDS) {
+		speed = (poa >> RT305X_ESW_SPD_S) & 1;
+	} else {
+		if (port == RT305X_ESW_NUM_PORTS - 1)
+			poa >>= 1;
+		speed = (poa >> RT305X_ESW_SPD_S) & 3;
+	}
+	switch (speed) {
+	case 0:
+		link->speed = SWITCH_PORT_SPEED_10;
+		break;
+	case 1:
+		link->speed = SWITCH_PORT_SPEED_100;
+		break;
+	case 2:
+	case 3: /* forced gige speed can be 2 or 3 */
+		link->speed = SWITCH_PORT_SPEED_1000;
+		break;
+	default:
+		link->speed = SWITCH_PORT_SPEED_UNKNOWN;
+		break;
+	}
+
+	return 0;
+}
+
+static int esw_get_port_bool(struct switch_dev *dev,
+			 const struct switch_attr *attr,
+			 struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int idx = val->port_vlan;
+	u32 x, reg, shift;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS)
+		return -EINVAL;
+
+	switch (attr->id) {
+	case RT305X_ESW_ATTR_PORT_DISABLE:
+		reg = RT305X_ESW_REG_POC0;
+		shift = RT305X_ESW_POC0_DIS_PORT_S;
+		break;
+	case RT305X_ESW_ATTR_PORT_DOUBLETAG:
+		reg = RT305X_ESW_REG_SGC2;
+		shift = RT305X_ESW_SGC2_DOUBLE_TAG_S;
+		break;
+	case RT305X_ESW_ATTR_PORT_UNTAG:
+		reg = RT305X_ESW_REG_POC2;
+		shift = RT305X_ESW_POC2_UNTAG_EN_S;
+		break;
+	case RT305X_ESW_ATTR_PORT_LAN:
+		reg = RT305X_ESW_REG_SGC2;
+		shift = RT305X_ESW_SGC2_LAN_PMAP_S;
+		if (idx >= RT305X_ESW_NUM_LANWAN)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	x = esw_r32(esw, reg);
+	val->value.i = (x >> (idx + shift)) & 1;
+
+	return 0;
+}
+
+static int esw_set_port_bool(struct switch_dev *dev,
+			 const struct switch_attr *attr,
+			 struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int idx = val->port_vlan;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS ||
+	    val->value.i < 0 || val->value.i > 1)
+		return -EINVAL;
+
+	switch (attr->id) {
+	case RT305X_ESW_ATTR_PORT_DISABLE:
+		esw->ports[idx].disable = val->value.i;
+		break;
+	case RT305X_ESW_ATTR_PORT_DOUBLETAG:
+		esw->ports[idx].doubletag = val->value.i;
+		break;
+	case RT305X_ESW_ATTR_PORT_UNTAG:
+		esw->ports[idx].untag = val->value.i;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int esw_get_port_recv_badgood(struct switch_dev *dev,
+				 const struct switch_attr *attr,
+				 struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int idx = val->port_vlan;
+	int shift = attr->id == RT305X_ESW_ATTR_PORT_RECV_GOOD ? 0 : 16;
+	u32 reg;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_LANWAN)
+		return -EINVAL;
+	reg = esw_r32(esw, RT305X_ESW_REG_PXPC(idx));
+	val->value.i = (reg >> shift) & 0xffff;
+
+	return 0;
+}
+
+static int
+esw_get_port_tr_badgood(struct switch_dev *dev,
+				 const struct switch_attr *attr,
+				 struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	int idx = val->port_vlan;
+	int shift = attr->id == RT5350_ESW_ATTR_PORT_TR_GOOD ? 0 : 16;
+	u32 reg;
+
+	if ((ralink_soc != RT305X_SOC_RT5350) && (ralink_soc != MT762X_SOC_MT7628AN) && (ralink_soc != MT762X_SOC_MT7688))
+		return -EINVAL;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_LANWAN)
+		return -EINVAL;
+
+	reg = esw_r32(esw, RT5350_ESW_REG_PXTPC(idx));
+	val->value.i = (reg >> shift) & 0xffff;
+
+	return 0;
+}
+
+static int esw_get_port_led(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int idx = val->port_vlan;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS ||
+	    idx >= RT305X_ESW_NUM_LEDS)
+		return -EINVAL;
+
+	val->value.i = esw_r32(esw, RT305X_ESW_REG_P0LED + 4*idx);
+
+	return 0;
+}
+
+static int esw_set_port_led(struct switch_dev *dev,
+			const struct switch_attr *attr,
+			struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int idx = val->port_vlan;
+
+	if (idx < 0 || idx >= RT305X_ESW_NUM_LEDS)
+		return -EINVAL;
+
+	esw->ports[idx].led = val->value.i;
+
+	return 0;
+}
+
+static int esw_get_port_pvid(struct switch_dev *dev, int port, int *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	if (port >= RT305X_ESW_NUM_PORTS)
+		return -EINVAL;
+
+	*val = esw_get_pvid(esw, port);
+
+	return 0;
+}
+
+static int esw_set_port_pvid(struct switch_dev *dev, int port, int val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+
+	if (port >= RT305X_ESW_NUM_PORTS)
+		return -EINVAL;
+
+	esw->ports[port].pvid = val;
+
+	return 0;
+}
+
+static int esw_get_vlan_ports(struct switch_dev *dev, struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	u32 vmsc, poc2;
+	int vlan_idx = -1;
+	int i;
+
+	val->len = 0;
+
+	if (val->port_vlan < 0 || val->port_vlan >= RT305X_ESW_NUM_VIDS)
+		return -EINVAL;
+
+	/* valid vlan? */
+	for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
+		if (esw_get_vlan_id(esw, i) == val->port_vlan &&
+		    esw_get_vmsc(esw, i) != RT305X_ESW_PORTS_NONE) {
+			vlan_idx = i;
+			break;
+		}
+	}
+
+	if (vlan_idx == -1)
+		return -EINVAL;
+
+	vmsc = esw_get_vmsc(esw, vlan_idx);
+	poc2 = esw_r32(esw, RT305X_ESW_REG_POC2);
+
+	for (i = 0; i < RT305X_ESW_NUM_PORTS; i++) {
+		struct switch_port *p;
+		int port_mask = 1 << i;
+
+		if (!(vmsc & port_mask))
+			continue;
+
+		p = &val->value.ports[val->len++];
+		p->id = i;
+		if (poc2 & (port_mask << RT305X_ESW_POC2_UNTAG_EN_S))
+			p->flags = 0;
+		else
+			p->flags = 1 << SWITCH_PORT_FLAG_TAGGED;
+	}
+
+	return 0;
+}
+
+static int esw_set_vlan_ports(struct switch_dev *dev, struct switch_val *val)
+{
+	struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
+	int ports;
+	int vlan_idx = -1;
+	int i;
+
+	if (val->port_vlan < 0 || val->port_vlan >= RT305X_ESW_NUM_VIDS ||
+	    val->len > RT305X_ESW_NUM_PORTS)
+		return -EINVAL;
+
+	/* one of the already defined vlans? */
+	for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
+		if (esw->vlans[i].vid == val->port_vlan &&
+		    esw->vlans[i].ports != RT305X_ESW_PORTS_NONE) {
+			vlan_idx = i;
+			break;
+		}
+	}
+
+	/* select a free slot */
+	for (i = 0; vlan_idx == -1 && i < RT305X_ESW_NUM_VLANS; i++) {
+		if (esw->vlans[i].ports == RT305X_ESW_PORTS_NONE)
+			vlan_idx = i;
+	}
+
+	/* bail if all slots are in use */
+	if (vlan_idx == -1)
+		return -EINVAL;
+
+	ports = RT305X_ESW_PORTS_NONE;
+	for (i = 0; i < val->len; i++) {
+		struct switch_port *p = &val->value.ports[i];
+		int port_mask = 1 << p->id;
+		bool untagged = !(p->flags & (1 << SWITCH_PORT_FLAG_TAGGED));
+
+		if (p->id >= RT305X_ESW_NUM_PORTS)
+			return -EINVAL;
+
+		ports |= port_mask;
+		esw->ports[p->id].untag = untagged;
+	}
+	esw->vlans[vlan_idx].ports = ports;
+	if (ports == RT305X_ESW_PORTS_NONE)
+		esw->vlans[vlan_idx].vid = RT305X_ESW_VLAN_NONE;
+	else
+		esw->vlans[vlan_idx].vid = val->port_vlan;
+
+	return 0;
+}
+
+static const struct switch_attr esw_global[] = {
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "enable_vlan",
+		.description = "VLAN mode (1:enabled)",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_ENABLE_VLAN,
+		.get = esw_get_vlan_enable,
+		.set = esw_set_vlan_enable,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "alternate_vlan_disable",
+		.description = "Use en_vlan instead of doubletag to disable"
+				" VLAN mode",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_ALT_VLAN_DISABLE,
+		.get = esw_get_alt_vlan_disable,
+		.set = esw_set_alt_vlan_disable,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "bc_storm_protect",
+		.description = "Global broadcast storm protection (0:Disable, 1:64 blocks, 2:96 blocks, 3:128 blocks)",
+		.max = 3,
+		.id = RT305X_ESW_ATTR_BC_STATUS,
+		.get = rt305x_esw_get_bc_status,
+		.set = rt305x_esw_set_bc_status,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "led_frequency",
+		.description = "LED Flash frequency (0:30mS, 1:60mS, 2:240mS, 3:480mS)",
+		.max = 3,
+		.id = RT305X_ESW_ATTR_LED_FREQ,
+		.get = rt305x_esw_get_led_freq,
+		.set = rt305x_esw_set_led_freq,
+	}
+};
+
+static const struct switch_attr esw_port[] = {
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "disable",
+		.description = "Port state (1:disabled)",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_PORT_DISABLE,
+		.get = esw_get_port_bool,
+		.set = esw_set_port_bool,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "doubletag",
+		.description = "Double tagging for incoming vlan packets "
+				"(1:enabled)",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_PORT_DOUBLETAG,
+		.get = esw_get_port_bool,
+		.set = esw_set_port_bool,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "untag",
+		.description = "Untag (1:strip outgoing vlan tag)",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_PORT_UNTAG,
+		.get = esw_get_port_bool,
+		.set = esw_set_port_bool,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "led",
+		.description = "LED mode (0:link, 1:100m, 2:duplex, 3:activity,"
+				" 4:collision, 5:linkact, 6:duplcoll, 7:10mact,"
+				" 8:100mact, 10:blink, 11:off, 12:on)",
+		.max = 15,
+		.id = RT305X_ESW_ATTR_PORT_LED,
+		.get = esw_get_port_led,
+		.set = esw_set_port_led,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "lan",
+		.description = "HW port group (0:wan, 1:lan)",
+		.max = 1,
+		.id = RT305X_ESW_ATTR_PORT_LAN,
+		.get = esw_get_port_bool,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "recv_bad",
+		.description = "Receive bad packet counter",
+		.id = RT305X_ESW_ATTR_PORT_RECV_BAD,
+		.get = esw_get_port_recv_badgood,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "recv_good",
+		.description = "Receive good packet counter",
+		.id = RT305X_ESW_ATTR_PORT_RECV_GOOD,
+		.get = esw_get_port_recv_badgood,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "tr_bad",
+
+		.description = "Transmit bad packet counter. rt5350 only",
+		.id = RT5350_ESW_ATTR_PORT_TR_BAD,
+		.get = esw_get_port_tr_badgood,
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "tr_good",
+
+		.description = "Transmit good packet counter. rt5350 only",
+		.id = RT5350_ESW_ATTR_PORT_TR_GOOD,
+		.get = esw_get_port_tr_badgood,
+	},
+};
+
+static const struct switch_attr esw_vlan[] = {
+};
+
+static const struct switch_dev_ops esw_ops = {
+	.attr_global = {
+		.attr = esw_global,
+		.n_attr = ARRAY_SIZE(esw_global),
+	},
+	.attr_port = {
+		.attr = esw_port,
+		.n_attr = ARRAY_SIZE(esw_port),
+	},
+	.attr_vlan = {
+		.attr = esw_vlan,
+		.n_attr = ARRAY_SIZE(esw_vlan),
+	},
+	.get_vlan_ports = esw_get_vlan_ports,
+	.set_vlan_ports = esw_set_vlan_ports,
+	.get_port_pvid = esw_get_port_pvid,
+	.set_port_pvid = esw_set_port_pvid,
+	.get_port_link = esw_get_port_link,
+	.apply_config = esw_apply_config,
+	.reset_switch = esw_reset_switch,
+};
+
 static int esw_probe(struct platform_device *pdev)
 {
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct device_node *np = pdev->dev.of_node;
 	const __be32 *port_map, *reg_init;
+	struct switch_dev *swdev;
 	struct rt305x_esw *esw;
 	struct resource *irq;
 	int ret;
@@ -568,6 +1353,21 @@ static int esw_probe(struct platform_device *pdev)
 	if (reg_init)
 		esw->reg_led_polarity = be32_to_cpu(*reg_init);
 
+	swdev = &esw->swdev;
+	swdev->of_node = pdev->dev.of_node;
+	swdev->name = "rt305x-esw";
+	swdev->alias = "rt305x";
+	swdev->cpu_port = RT305X_ESW_PORT6;
+	swdev->ports = RT305X_ESW_NUM_PORTS;
+	swdev->vlans = RT305X_ESW_NUM_VIDS;
+	swdev->ops = &esw_ops;
+
+	ret = register_switch(swdev, NULL);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "register_switch failed\n");
+		goto unmap_base;
+	}
+
 	platform_set_drvdata(pdev, esw);
 
 	spin_lock_init(&esw->reg_rw_lock);
@@ -583,6 +1383,11 @@ static int esw_probe(struct platform_device *pdev)
 	}
 
 	return ret;
+
+unmap_base:
+	iounmap(esw->base);
+	kfree(esw);
+	return ret;
 }
 
 static int esw_remove(struct platform_device *pdev)
-- 
1.7.10.4