ramips: Add swconfig support to ramips_esw.c
authorGabor Juhos <juhosg@openwrt.org>
Wed, 29 Aug 2012 10:37:36 +0000 (10:37 +0000)
committerGabor Juhos <juhosg@openwrt.org>
Wed, 29 Aug 2012 10:37:36 +0000 (10:37 +0000)
Add swconfig support to ramips_esw.c

This patch adds swconfig support for ramips_esw:

Tested on both D-LINK DIR-300 B1 and Sitecom WL-351 (external
rtl8366rb on internal port 5).
I've made sure that in the enable_vlan=0 case it behaves like a dumb
switch, so external switches should work fine with vlans and
verified this on the WL-351.

The current state shown by swconfig is always read directly from HW
registers, new settings only show after 'swconfig dev rt305x set apply'.

Signed-off-by: Tobias Diedrich <ranma+openwrt@tdiedrich.de>
SVN-Revision: 33299

target/linux/ramips/files/drivers/net/ethernet/ramips/Kconfig
target/linux/ramips/files/drivers/net/ethernet/ramips/ramips_esw.c

index c821d5b..1bc4c2b 100644 (file)
@@ -2,6 +2,7 @@ config NET_RAMIPS
        tristate "Ralink RT288X/RT3X5X/RT3662/RT3883 ethernet driver"
        depends on MIPS_RALINK
        select PHYLIB if (SOC_RT288X || SOC_RT3883)
+       select SWCONFIG if SOC_RT305X
        help
          This driver supports the etehrnet mac inside the ralink wisocs
 
index 6880368..8a70795 100644 (file)
@@ -1,19 +1,33 @@
 #include <linux/ioport.h>
+#include <linux/switch.h>
 
 #include <rt305x_regs.h>
 #include <rt305x_esw_platform.h>
 
+/*
+ * HW limitations for this switch:
+ * - No large frame support (PKT_MAX_LEN at most 1536)
+ * - Can't have untagged vlan and tagged vlan on one port at the same time,
+ *   though this might be possible using the undocumented PPE.
+ */
+
 #define RT305X_ESW_REG_FCT0            0x08
 #define RT305X_ESW_REG_PFC1            0x14
+#define RT305X_ESW_REG_ATS             0x24
+#define RT305X_ESW_REG_ATS0            0x28
+#define RT305X_ESW_REG_ATS1            0x2c
+#define RT305X_ESW_REG_ATS2            0x30
 #define RT305X_ESW_REG_PVIDC(_n)       (0x40 + 4 * (_n))
 #define RT305X_ESW_REG_VLANI(_n)       (0x50 + 4 * (_n))
 #define RT305X_ESW_REG_VMSC(_n)                (0x70 + 4 * (_n))
+#define RT305X_ESW_REG_POA             0x80
 #define RT305X_ESW_REG_FPA             0x84
 #define RT305X_ESW_REG_SOCPC           0x8c
 #define RT305X_ESW_REG_POC1            0x90
 #define RT305X_ESW_REG_POC2            0x94
 #define RT305X_ESW_REG_POC3            0x98
 #define RT305X_ESW_REG_SGC             0x9c
+#define RT305X_ESW_REG_STRT            0xa0
 #define RT305X_ESW_REG_PCR0            0xc0
 #define RT305X_ESW_REG_PCR1            0xc4
 #define RT305X_ESW_REG_FPA2            0xc8
 #define RT305X_ESW_REG_P2LED           0xac
 #define RT305X_ESW_REG_P3LED           0xb0
 #define RT305X_ESW_REG_P4LED           0xb4
+#define RT305X_ESW_REG_P0PC            0xe8
+#define RT305X_ESW_REG_P1PC            0xec
+#define RT305X_ESW_REG_P2PC            0xf0
+#define RT305X_ESW_REG_P3PC            0xf4
+#define RT305X_ESW_REG_P4PC            0xf8
+#define RT305X_ESW_REG_P5PC            0xfc
+
+#define RT305X_ESW_LED_LINK            0
+#define RT305X_ESW_LED_100M            1
+#define RT305X_ESW_LED_DUPLEX          2
+#define RT305X_ESW_LED_ACTIVITY                3
+#define RT305X_ESW_LED_COLLISION       4
+#define RT305X_ESW_LED_LINKACT         5
+#define RT305X_ESW_LED_DUPLCOLL                6
+#define RT305X_ESW_LED_10MACT          7
+#define RT305X_ESW_LED_100MACT         8
+/* Additional led states not in datasheet: */
+#define RT305X_ESW_LED_BLINK           10
+#define RT305X_ESW_LED_ON              12
+
+#define RT305X_ESW_LINK_S              25
+#define RT305X_ESW_DUPLEX_S            9
+#define RT305X_ESW_SPD_S               0
 
 #define RT305X_ESW_PCR0_WT_NWAY_DATA_S 16
 #define RT305X_ESW_PCR0_WT_PHY_CMD     BIT(13)
@@ -31,6 +68,7 @@
 
 #define RT305X_ESW_PCR1_WT_DONE                BIT(0)
 
+#define RT305X_ESW_ATS_TIMEOUT         (5 * HZ)
 #define RT305X_ESW_PHY_TIMEOUT         (5 * HZ)
 
 #define RT305X_ESW_PVIDC_PVID_M                0xfff
 #define RT305X_ESW_POC1_EN_BP_S                0
 #define RT305X_ESW_POC1_EN_FC_S                8
 #define RT305X_ESW_POC1_DIS_RMC2CPU_S  16
+#define RT305X_ESW_POC1_DIS_PORT_M     0x7f
 #define RT305X_ESW_POC1_DIS_PORT_S     23
 
+#define RT305X_ESW_POC3_UNTAG_EN_M     0xff
 #define RT305X_ESW_POC3_UNTAG_EN_S     0
 #define RT305X_ESW_POC3_ENAGING_S      8
 #define RT305X_ESW_POC3_DIS_UC_PAUSE_S 16
 
+#define RT305X_ESW_SGC2_DOUBLE_TAG_M   0x7f
+#define RT305X_ESW_SGC2_DOUBLE_TAG_S   0
+#define RT305X_ESW_SGC2_LAN_PMAP_M     0x3f
+#define RT305X_ESW_SGC2_LAN_PMAP_S     24
+
+#define RT305X_ESW_PFC1_EN_VLAN_M      0xff
+#define RT305X_ESW_PFC1_EN_VLAN_S      16
+#define RT305X_ESW_PFC1_EN_TOS_S       24
+
+#define RT305X_ESW_VLAN_NONE           0xfff
+
 #define RT305X_ESW_PORT0               0
 #define RT305X_ESW_PORT1               1
 #define RT305X_ESW_PORT2               2
 #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
+
 #define RT305X_ESW_PORTS_INTERNAL                                      \
                (BIT(RT305X_ESW_PORT0) | BIT(RT305X_ESW_PORT1) |        \
                 BIT(RT305X_ESW_PORT2) | BIT(RT305X_ESW_PORT3) |        \
                (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
+
+enum {
+       /* Global attributes. */
+       RT305X_ESW_ATTR_ENABLE_VLAN,
+       RT305X_ESW_ATTR_ALT_VLAN_DISABLE,
+       /* Port attributes. */
+       RT305X_ESW_ATTR_PORT_DISABLE,
+       RT305X_ESW_ATTR_PORT_DOUBLETAG,
+       RT305X_ESW_ATTR_PORT_EN_VLAN,
+       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,
+};
+
+struct rt305x_esw_port {
+       bool    disable;
+       bool    doubletag;
+       bool    untag;
+       bool    en_vlan;
+       u8      led;
+       u16     pvid;
+};
+
+struct rt305x_esw_vlan {
+       u8      ports;
+       u16     vid;
+};
 
 struct rt305x_esw {
        void __iomem            *base;
        struct rt305x_esw_platform_data *pdata;
+       /* Protects against concurrent register rmw operations. */
        spinlock_t              reg_rw_lock;
+
+       struct switch_dev       swdev;
+       bool                    global_vlan_enable;
+       bool                    alt_vlan_disable;
+       struct rt305x_esw_vlan vlans[RT305X_ESW_NUM_VLANS];
+       struct rt305x_esw_port ports[RT305X_ESW_NUM_PORTS];
+
 };
 
 static inline void
@@ -160,6 +257,19 @@ out:
        return ret;
 }
 
+static unsigned
+rt305x_esw_get_vlan_id(struct rt305x_esw *esw, unsigned vlan)
+{
+       unsigned s;
+       unsigned val;
+
+       s = RT305X_ESW_VLANI_VID_S * (vlan % 2);
+       val = rt305x_esw_rr(esw, RT305X_ESW_REG_VLANI(vlan / 2));
+       val = (val >> s) & RT305X_ESW_VLANI_VID_M;
+
+       return val;
+}
+
 static void
 rt305x_esw_set_vlan_id(struct rt305x_esw *esw, unsigned vlan, unsigned vid)
 {
@@ -172,6 +282,16 @@ rt305x_esw_set_vlan_id(struct rt305x_esw *esw, unsigned vlan, unsigned vid)
                       (vid & RT305X_ESW_VLANI_VID_M) << s);
 }
 
+static unsigned
+rt305x_esw_get_pvid(struct rt305x_esw *esw, unsigned port)
+{
+       unsigned s, val;
+
+       s = RT305X_ESW_PVIDC_PVID_S * (port % 2);
+       val = rt305x_esw_rr(esw, RT305X_ESW_REG_PVIDC(port / 2));
+       return (val >> s) & RT305X_ESW_PVIDC_PVID_M;
+}
+
 static void
 rt305x_esw_set_pvid(struct rt305x_esw *esw, unsigned port, unsigned pvid)
 {
@@ -184,6 +304,18 @@ rt305x_esw_set_pvid(struct rt305x_esw *esw, unsigned port, unsigned pvid)
                       (pvid & RT305X_ESW_PVIDC_PVID_M) << s);
 }
 
+static unsigned
+rt305x_esw_get_vmsc(struct rt305x_esw *esw, unsigned vlan)
+{
+       unsigned s, val;
+
+       s = RT305X_ESW_VMSC_MSC_S * (vlan % 4);
+       val = rt305x_esw_rr(esw, RT305X_ESW_REG_VMSC(vlan / 4));
+       val = (val >> s) & RT305X_ESW_VMSC_MSC_M;
+
+       return val;
+}
+
 static void
 rt305x_esw_set_vmsc(struct rt305x_esw *esw, unsigned vlan, unsigned msc)
 {
@@ -196,15 +328,22 @@ rt305x_esw_set_vmsc(struct rt305x_esw *esw, unsigned vlan, unsigned msc)
                       (msc & RT305X_ESW_VMSC_MSC_M) << s);
 }
 
+static int
+rt305x_esw_apply_config(struct switch_dev *dev);
+
 static void
 rt305x_esw_hw_init(struct rt305x_esw *esw)
 {
        int i;
+       u8 port_map = 0;
 
        /* vodoo from original driver */
        rt305x_esw_wr(esw, 0xC8A07850, RT305X_ESW_REG_FCT0);
        rt305x_esw_wr(esw, 0x00000000, RT305X_ESW_REG_SGC2);
-       rt305x_esw_wr(esw, 0x00405555, RT305X_ESW_REG_PFC1);
+       /* Port priority 1 for all ports, vlan enabled. */
+       rt305x_esw_wr(esw, 0x00005555 |
+                     (RT305X_ESW_PORTS_ALL << RT305X_ESW_PFC1_EN_VLAN_S),
+                     RT305X_ESW_REG_PFC1);
 
        /* Enable Back Pressure, and Flow Control */
        rt305x_esw_wr(esw,
@@ -219,6 +358,14 @@ rt305x_esw_hw_init(struct rt305x_esw *esw)
                      RT305X_ESW_REG_POC3);
 
        rt305x_esw_wr(esw, esw->pdata->reg_initval_fct2, RT305X_ESW_REG_FCT2);
+
+       /*
+        * 300s aging timer, max packet len 1536, broadcast storm prevention
+        * disabled, disable collision abort, mac xor48 hash, 10 packet back
+        * pressure jam, GMII disable was_transmit, back pressure disabled,
+        * 30ms led flash, unmatched IGMP as broadcast, rmc tb fault to all
+        * ports.
+        */
        rt305x_esw_wr(esw, 0x0008a301, RT305X_ESW_REG_SGC);
 
        /* Setup SoC Port control register */
@@ -265,66 +412,621 @@ rt305x_esw_hw_init(struct rt305x_esw *esw)
        /* select local register */
        rt305x_mii_write(esw, 0, 31, 0x8000);
 
+       /* Set up logical config and apply. */
        for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
-               rt305x_esw_set_vlan_id(esw, i, 0);
-               rt305x_esw_set_vmsc(esw, i, 0);
+               esw->vlans[i].vid = RT305X_ESW_VLAN_NONE;
+               esw->vlans[i].ports = RT305X_ESW_PORTS_NONE;
        }
 
-       for (i = 0; i < RT305X_ESW_NUM_PORTS; i++)
-               rt305x_esw_set_pvid(esw, i, 1);
+       for (i = 0; i < RT305X_ESW_NUM_PORTS; i++) {
+               esw->ports[i].pvid = 1;
+               esw->ports[i].en_vlan = 1;
+               esw->ports[i].untag = i != RT305X_ESW_PORT6;
+       }
 
        switch (esw->pdata->vlan_config) {
-       case RT305X_ESW_VLAN_CONFIG_NONE:
-               break;
-
        case RT305X_ESW_VLAN_CONFIG_BYPASS:
-               /* Pass all vlan tags to all ports */
-               for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
-                       rt305x_esw_set_vlan_id(esw, i, i+1);
-                       rt305x_esw_set_vmsc(esw, i, RT305X_ESW_PORTS_ALL);
-               }
-               /* Disable VLAN TAG removal, keep aging on. */
-               rt305x_esw_wr(esw,
-                             RT305X_ESW_PORTS_ALL << RT305X_ESW_POC3_ENAGING_S,
-                             RT305X_ESW_REG_POC3);
+       case RT305X_ESW_VLAN_CONFIG_NONE:
+               port_map = RT305X_ESW_PMAP_LLLLLL;
+               esw->global_vlan_enable = 0;
                break;
 
        case RT305X_ESW_VLAN_CONFIG_LLLLW:
-               rt305x_esw_set_vlan_id(esw, 0, 1);
-               rt305x_esw_set_vlan_id(esw, 1, 2);
-               rt305x_esw_set_pvid(esw, RT305X_ESW_PORT4, 2);
-
-               rt305x_esw_set_vmsc(esw, 0,
+               port_map = RT305X_ESW_PMAP_LLLLWL;
+               esw->global_vlan_enable = 1;
+               esw->vlans[0].vid = 1;
+               esw->vlans[1].vid = 2;
+               esw->ports[4].pvid = 2;
+               esw->ports[5].disable = 1;
+               esw->vlans[0].ports =
                                BIT(RT305X_ESW_PORT0) | BIT(RT305X_ESW_PORT1) |
                                BIT(RT305X_ESW_PORT2) | BIT(RT305X_ESW_PORT3) |
-                               BIT(RT305X_ESW_PORT6));
-               rt305x_esw_set_vmsc(esw, 1,
-                               BIT(RT305X_ESW_PORT4) | BIT(RT305X_ESW_PORT6));
+                               BIT(RT305X_ESW_PORT6);
+               esw->vlans[1].ports =
+                               BIT(RT305X_ESW_PORT4) | BIT(RT305X_ESW_PORT6);
                break;
 
        case RT305X_ESW_VLAN_CONFIG_WLLLL:
-               rt305x_esw_set_vlan_id(esw, 0, 1);
-               rt305x_esw_set_vlan_id(esw, 1, 2);
-               rt305x_esw_set_pvid(esw, RT305X_ESW_PORT0, 2);
-
-               rt305x_esw_set_vmsc(esw, 0,
-                               BIT(RT305X_ESW_PORT1) | BIT(RT305X_ESW_PORT2) |
-                               BIT(RT305X_ESW_PORT3) | BIT(RT305X_ESW_PORT4) |
-                               BIT(RT305X_ESW_PORT6));
-               rt305x_esw_set_vmsc(esw, 1,
-                               BIT(RT305X_ESW_PORT0) | BIT(RT305X_ESW_PORT6));
+               port_map = RT305X_ESW_PMAP_WLLLLL;
+               esw->global_vlan_enable = 1;
+               esw->vlans[0].vid = 1;
+               esw->vlans[1].vid = 2;
+               esw->ports[0].pvid = 2;
+               esw->ports[5].disable = 1;
+               esw->vlans[0].ports =
+                       BIT(RT305X_ESW_PORT1) | BIT(RT305X_ESW_PORT2) |
+                       BIT(RT305X_ESW_PORT3) | BIT(RT305X_ESW_PORT4) |
+                       BIT(RT305X_ESW_PORT6);
+               esw->vlans[1].ports =
+                               BIT(RT305X_ESW_PORT0) | BIT(RT305X_ESW_PORT6);
                break;
 
        default:
                BUG();
        }
+
+       /*
+        * Unused HW feature, but still nice to be consistent here...
+        * This is also exported to userspace ('lan' attribute) so it's
+        * conveniently usable to decide which ports go into the wan vlan by
+        * default.
+        */
+       rt305x_esw_rmw(esw, RT305X_ESW_REG_SGC2,
+                      RT305X_ESW_SGC2_LAN_PMAP_M << RT305X_ESW_SGC2_LAN_PMAP_S,
+                      port_map << RT305X_ESW_SGC2_LAN_PMAP_S);
+
+       rt305x_esw_apply_config(&esw->swdev);
+}
+
+static int
+rt305x_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;
+               }
+               rt305x_esw_set_vlan_id(esw, i, vid);
+               rt305x_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   |= esw->ports[i].en_vlan   << i;
+                       untag     |= esw->ports[i].untag     << i;
+                       pvid       = esw->ports[i].pvid;
+               } else {
+                       int x = esw->alt_vlan_disable ? 1 : 0;
+                       doubletag |= x << i;
+                       en_vlan   |= x << i;
+                       untag     |= x << i;
+                       pvid       = 0;
+               }
+               rt305x_esw_set_pvid(esw, i, pvid);
+               if (i < RT305X_ESW_NUM_LEDS)
+                       rt305x_esw_wr(esw, esw->ports[i].led,
+                                     RT305X_ESW_REG_P0LED + 4*i);
+       }
+
+       rt305x_esw_rmw(esw, RT305X_ESW_REG_POC1,
+                      RT305X_ESW_POC1_DIS_PORT_M << RT305X_ESW_POC1_DIS_PORT_S,
+                      disable << RT305X_ESW_POC1_DIS_PORT_S);
+       rt305x_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);
+       rt305x_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);
+       rt305x_esw_rmw(esw, RT305X_ESW_REG_POC3,
+                      RT305X_ESW_POC3_UNTAG_EN_M << RT305X_ESW_POC3_UNTAG_EN_S,
+                      untag << RT305X_ESW_POC3_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
+                */
+               rt305x_esw_set_vlan_id(esw, 0, 0);
+               rt305x_esw_set_vmsc(esw, 0, RT305X_ESW_PORTS_ALL);
+       }
+
+       return 0;
+}
+
+static int
+rt305x_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));
+       rt305x_esw_hw_init(esw);
+
+       return 0;
+}
+
+static int
+rt305x_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
+rt305x_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
+rt305x_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
+rt305x_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_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 = rt305x_esw_rr(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
+rt305x_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_POC1;
+               shift = RT305X_ESW_POC1_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_EN_VLAN:
+               reg = RT305X_ESW_REG_PFC1;
+               shift = RT305X_ESW_PFC1_EN_VLAN_S;
+               break;
+       case RT305X_ESW_ATTR_PORT_UNTAG:
+               reg = RT305X_ESW_REG_POC3;
+               shift = RT305X_ESW_POC3_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 = rt305x_esw_rr(esw, reg);
+       val->value.i = (x >> (idx + shift)) & 1;
+
+       return 0;
+}
+
+static int
+rt305x_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_EN_VLAN:
+               esw->ports[idx].en_vlan = 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
+rt305x_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;
+
+       if (idx < 0 || idx >= RT305X_ESW_NUM_LANWAN)
+               return -EINVAL;
+
+       val->value.i = rt305x_esw_rr(esw, RT305X_ESW_REG_P0PC + 4*idx) >> shift;
+
+       return 0;
+}
+
+static int
+rt305x_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 = rt305x_esw_rr(esw, RT305X_ESW_REG_P0LED + 4*idx);
+
+       return 0;
+}
+
+static int
+rt305x_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
+rt305x_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 = rt305x_esw_get_pvid(esw, port);
+
+       return 0;
+}
+
+static int
+rt305x_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
+rt305x_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, poc3;
+       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 (rt305x_esw_get_vlan_id(esw, i) == val->port_vlan &&
+                   rt305x_esw_get_vmsc(esw, i) != RT305X_ESW_PORTS_NONE) {
+                       vlan_idx = i;
+                       break;
+               }
+       }
+
+       if (vlan_idx == -1)
+               return -EINVAL;
+
+       vmsc = rt305x_esw_get_vmsc(esw, vlan_idx);
+       poc3 = rt305x_esw_rr(esw, RT305X_ESW_REG_POC3);
+
+       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 (poc3 & (port_mask << RT305X_ESW_POC3_UNTAG_EN_S))
+                       p->flags = 0;
+               else
+                       p->flags = 1 << SWITCH_PORT_FLAG_TAGGED;
+       }
+
+       return 0;
+}
+
+static int
+rt305x_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 rt305x_esw_global[] = {
+       {
+               .type = SWITCH_TYPE_INT,
+               .name = "enable_vlan",
+               .description = "VLAN mode (1:enabled)",
+               .max = 1,
+               .id = RT305X_ESW_ATTR_ENABLE_VLAN,
+               .get = rt305x_esw_get_vlan_enable,
+               .set = rt305x_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 = rt305x_esw_get_alt_vlan_disable,
+               .set = rt305x_esw_set_alt_vlan_disable,
+       },
+};
+
+static const struct switch_attr rt305x_esw_port[] = {
+       {
+               .type = SWITCH_TYPE_INT,
+               .name = "disable",
+               .description = "Port state (1:disabled)",
+               .max = 1,
+               .id = RT305X_ESW_ATTR_PORT_DISABLE,
+               .get = rt305x_esw_get_port_bool,
+               .set = rt305x_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 = rt305x_esw_get_port_bool,
+               .set = rt305x_esw_set_port_bool,
+       },
+       {
+               .type = SWITCH_TYPE_INT,
+               .name = "en_vlan",
+               .description = "VLAN enabled (1:enabled)",
+               .max = 1,
+               .id = RT305X_ESW_ATTR_PORT_EN_VLAN,
+               .get = rt305x_esw_get_port_bool,
+               .set = rt305x_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 = rt305x_esw_get_port_bool,
+               .set = rt305x_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, 12:on)",
+               .max = 15,
+               .id = RT305X_ESW_ATTR_PORT_LED,
+               .get = rt305x_esw_get_port_led,
+               .set = rt305x_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 = rt305x_esw_get_port_bool,
+       },
+       {
+               .type = SWITCH_TYPE_INT,
+               .name = "recv_bad",
+               .description = "Receive bad packet counter",
+               .id = RT305X_ESW_ATTR_PORT_RECV_BAD,
+               .get = rt305x_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 = rt305x_esw_get_port_recv_badgood,
+       },
+};
+
+static const struct switch_attr rt305x_esw_vlan[] = {
+};
+
+static const struct switch_dev_ops rt305x_esw_ops = {
+       .attr_global = {
+               .attr = rt305x_esw_global,
+               .n_attr = ARRAY_SIZE(rt305x_esw_global),
+       },
+       .attr_port = {
+               .attr = rt305x_esw_port,
+               .n_attr = ARRAY_SIZE(rt305x_esw_port),
+       },
+       .attr_vlan = {
+               .attr = rt305x_esw_vlan,
+               .n_attr = ARRAY_SIZE(rt305x_esw_vlan),
+       },
+       .get_vlan_ports = rt305x_esw_get_vlan_ports,
+       .set_vlan_ports = rt305x_esw_set_vlan_ports,
+       .get_port_pvid = rt305x_esw_get_port_pvid,
+       .set_port_pvid = rt305x_esw_set_port_pvid,
+       .get_port_link = rt305x_esw_get_port_link,
+       .apply_config = rt305x_esw_apply_config,
+       .reset_switch = rt305x_esw_reset_switch,
+};
+
 static int
 rt305x_esw_probe(struct platform_device *pdev)
 {
        struct rt305x_esw_platform_data *pdata;
        struct rt305x_esw *esw;
+       struct switch_dev *swdev;
        struct resource *res;
        int err;
 
@@ -351,6 +1053,20 @@ rt305x_esw_probe(struct platform_device *pdev)
                goto free_esw;
        }
 
+       swdev = &esw->swdev;
+       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 = &rt305x_esw_ops;
+
+       err = register_switch(swdev, NULL);
+       if (err < 0) {
+               dev_err(&pdev->dev, "register_switch failed\n");
+               goto unmap_base;
+       }
+
        platform_set_drvdata(pdev, esw);
 
        esw->pdata = pdata;
@@ -359,6 +1075,8 @@ rt305x_esw_probe(struct platform_device *pdev)
 
        return 0;
 
+unmap_base:
+       iounmap(esw->base);
 free_esw:
        kfree(esw);
        return err;
@@ -371,6 +1089,7 @@ rt305x_esw_remove(struct platform_device *pdev)
 
        esw = platform_get_drvdata(pdev);
        if (esw) {
+               unregister_switch(&esw->swdev);
                platform_set_drvdata(pdev, NULL);
                iounmap(esw->base);
                kfree(esw);