generic: rtl8366: update vlan handling code for rtl8366s

SVN-Revision: 21922

diff --git a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c
index 3bb0b549db11295bb0e785306bbd4cd2025bcd78..4e758e97c9a0a836348e401479f25256866f5a31 100644 (file)
--- a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c
+++ b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c
@@ -630,8 +630,7 @@ static int rtl8366s_set_vlan_mc(struct rtl8366_smi *smi, u32 index,
        return 0;
 }
 
        return 0;
 }
 

-static int rtl8366s_get_port_vlan_index(struct rtl8366_smi *smi, int port,
-                                      int *val)
+static int rtl8366s_get_mc_index(struct rtl8366_smi *smi, int port, int *val)

 {
        u32 data;
        int err;
 {
        u32 data;
        int err;


@@ -648,17 +647,68 @@ static int rtl8366s_get_port_vlan_index(struct rtl8366_smi *smi, int port,
               RTL8366S_PORT_VLAN_CTRL_MASK;
 
        return 0;
               RTL8366S_PORT_VLAN_CTRL_MASK;
 
        return 0;

+}
+
+static int rtl8366s_set_mc_index(struct rtl8366_smi *smi, int port, int index)
+{
+       if (port >= RTL8366_NUM_PORTS || index >= RTL8366_NUM_VLANS)
+               return -EINVAL;
+
+       return rtl8366_smi_rmwr(smi, RTL8366S_PORT_VLAN_CTRL_REG(port),
+                               RTL8366S_PORT_VLAN_CTRL_MASK <<
+                                       RTL8366S_PORT_VLAN_CTRL_SHIFT(port),
+                               (index & RTL8366S_PORT_VLAN_CTRL_MASK) <<
+                                       RTL8366S_PORT_VLAN_CTRL_SHIFT(port));
+}
+
+static int rtl8366s_set_vlan(struct rtl8366_smi *smi, int vid, u32 member,
+                            u32 untag, u32 fid)
+{
+       struct rtl8366_vlan_4k vlan4k;
+       int err;
+       int i;
+
+       /* Update the 4K table */
+       err = rtl8366s_get_vlan_4k(smi, vid, &vlan4k);
+       if (err)
+               return err;
+
+       vlan4k.member = member;
+       vlan4k.untag = untag;
+       vlan4k.fid = fid;
+       err = rtl8366s_set_vlan_4k(smi, &vlan4k);
+       if (err)
+               return err;
+
+       /* Try to find an existing MC entry for this VID */
+       for (i = 0; i < RTL8366_NUM_VLANS; i++) {
+               struct rtl8366_vlan_mc vlanmc;
+
+               err = rtl8366s_get_vlan_mc(smi, i, &vlanmc);
+               if (err)
+                       return err;

 
 

+               if (vid == vlanmc.vid) {
+                       /* update the MC entry */
+                       vlanmc.member = member;
+                       vlanmc.untag = untag;
+                       vlanmc.fid = fid;
+
+                       err = rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+                       break;
+               }
+       }
+
+       return err;

 }
 
 }
 

-static int rtl8366s_get_vlan_port_pvid(struct rtl8366_smi *smi, int port,
-                                      int *val)
+static int rtl8366s_get_pvid(struct rtl8366_smi *smi, int port, int *val)

 {
        struct rtl8366_vlan_mc vlanmc;
        int err;
        int index;
 
 {
        struct rtl8366_vlan_mc vlanmc;
        int err;
        int index;
 

-       err = rtl8366s_get_port_vlan_index(smi, port, &index);
+       err = rtl8366s_get_mc_index(smi, port, &index);

        if (err)
                return err;
 
        if (err)
                return err;
 


@@ -670,87 +720,111 @@ static int rtl8366s_get_vlan_port_pvid(struct rtl8366_smi *smi, int port,
        return 0;
 }
 
        return 0;
 }
 

-static int rtl8366s_set_port_vlan_index(struct rtl8366_smi *smi, int port,
-                                       int index)
+static int rtl8366s_mc_is_used(struct rtl8366_smi *smi, int mc_index,
+                              int *used)

 {
 {

-       if (port >= RTL8366_NUM_PORTS || index >= RTL8366_NUM_VLANS)
-               return -EINVAL;
+       int err;
+       int i;

 
 

-       return rtl8366_smi_rmwr(smi, RTL8366S_PORT_VLAN_CTRL_REG(port),
-                               RTL8366S_PORT_VLAN_CTRL_MASK <<
-                                       RTL8366S_PORT_VLAN_CTRL_SHIFT(port),
-                               (index & RTL8366S_PORT_VLAN_CTRL_MASK) <<
-                                       RTL8366S_PORT_VLAN_CTRL_SHIFT(port));
+       *used = 0;
+       for (i = 0; i < RTL8366_NUM_PORTS; i++) {
+               int index = 0;
+
+               err = rtl8366s_get_mc_index(smi, i, &index);
+               if (err)
+                       return err;
+
+               if (mc_index == index) {
+                       *used = 1;
+                       break;
+               }
+       }
+
+       return 0;

 }
 
 }
 

-static int rtl8366s_set_vlan_port_pvid(struct rtl8366_smi *smi, int port, int val)
+static int rtl8366s_set_pvid(struct rtl8366_smi *smi, unsigned port,
+                            unsigned vid)

 {
 {

-       int i;

        struct rtl8366_vlan_mc vlanmc;
        struct rtl8366_vlan_4k vlan4k;
        struct rtl8366_vlan_mc vlanmc;
        struct rtl8366_vlan_4k vlan4k;

+       int err;
+       int i;

 
 

-       if (port >= RTL8366_NUM_PORTS || val >= RTL8366_NUM_VIDS)
-               return -EINVAL;
-
-       /* Updating the 4K entry; lookup it and change the port member set */
-       rtl8366s_get_vlan_4k(smi, val, &vlan4k);
-       vlan4k.member |= ((1 << port) | RTL8366_PORT_CPU);
-       vlan4k.untag = RTL8366_PORT_ALL_BUT_CPU;
-       rtl8366s_set_vlan_4k(smi, &vlan4k);
-
-       /*
-        * For the 16 entries more work needs to be done. First see if such
-        * VID is already there and change it
-        */
-       for (i = 0; i < RTL8366_NUM_VLANS; ++i) {
-               rtl8366s_get_vlan_mc(smi, i, &vlanmc);
-
-               /* Try to find an existing vid and update port member set */
-               if (val == vlanmc.vid) {
-                       vlanmc.member |= ((1 << port) | RTL8366_PORT_CPU);
-                       rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+       /* Try to find an existing MC entry for this VID */
+       for (i = 0; i < RTL8366_NUM_VLANS; i++) {
+               err = rtl8366s_get_vlan_mc(smi, i, &vlanmc);
+               if (err)
+                       return err;

 
 

-                       /* Now update PVID register settings */
-                       rtl8366s_set_port_vlan_index(smi, port, i);
+               if (vid == vlanmc.vid) {
+                       err = rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+                       if (err)
+                               return err;

 
 

-                       return 0;
+                       err = rtl8366s_set_mc_index(smi, port, i);
+                       return err;

                }
        }
 
                }
        }
 

-       /*
-         * PVID could not be found from vlan table. Replace unused (one that
-        * has no member ports) with new one
-        */
-       for (i = 0; i < RTL8366_NUM_VLANS; ++i) {
-               rtl8366s_get_vlan_mc(smi, i, &vlanmc);
+       /* We have no MC entry for this VID, try to find an empty one */
+       for (i = 0; i < RTL8366_NUM_VLANS; i++) {
+               err = rtl8366s_get_vlan_mc(smi, i, &vlanmc);
+               if (err)
+                       return err;

 
 

-               /*
-                * See if this vlan member configuration is unused. It is
-                * unused if member set contains no ports or CPU port only
-                */
-               if (!vlanmc.member || vlanmc.member == RTL8366_PORT_CPU) {
-                       vlanmc.vid = val;
-                       vlanmc.priority = 0;
-                       vlanmc.untag = RTL8366_PORT_ALL_BUT_CPU;
-                       vlanmc.member = ((1 << port) | RTL8366_PORT_CPU);
-                       vlanmc.fid = 0;
+               if (vlanmc.vid == 0 && vlanmc.member == 0) {
+                       /* Update the entry from the 4K table */
+                       err = rtl8366s_get_vlan_4k(smi, vid, &vlan4k);
+                       if (err)
+                               return err;
+
+                       vlanmc.vid = vid;
+                       vlanmc.member = vlan4k.member;
+                       vlanmc.untag = vlan4k.untag;
+                       vlanmc.fid = vlan4k.fid;
+                       err = rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+                       if (err)
+                               return err;
+
+                       err = rtl8366s_set_mc_index(smi, port, i);
+                       return err;
+               }
+       }

 
 

-                       rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+       /* MC table is full, try to find an unused entry and replace it */
+       for (i = 0; i < RTL8366_NUM_VLANS; i++) {
+               int used;

 
 

-                       /* Now update PVID register settings */
-                       rtl8366s_set_port_vlan_index(smi, port, i);
+               err = rtl8366s_mc_is_used(smi, i, &used);
+               if (err)
+                       return err;

 
 

-                       return 0;
+               if (!used) {
+                       /* Update the entry from the 4K table */
+                       err = rtl8366s_get_vlan_4k(smi, vid, &vlan4k);
+                       if (err)
+                               return err;
+
+                       vlanmc.vid = vid;
+                       vlanmc.member = vlan4k.member;
+                       vlanmc.untag = vlan4k.untag;
+                       vlanmc.fid = vlan4k.fid;
+                       err = rtl8366s_set_vlan_mc(smi, i, &vlanmc);
+                       if (err)
+                               return err;
+
+                       err = rtl8366s_set_mc_index(smi, port, i);
+                       return err;

                }
        }
 
        dev_err(smi->parent,
                }
        }
 
        dev_err(smi->parent,

-               "All 16 vlan member configurations are in use\n");
+               "all VLAN member configurations are in use\n");

 
 

-       return -EINVAL;
+       return -ENOSPC;

 }
 
 }
 

-

 static int rtl8366s_vlan_set_vlan(struct rtl8366_smi *smi, int enable)
 {
        return rtl8366_smi_rmwr(smi, RTL8366_CHIP_GLOBAL_CTRL_REG,
 static int rtl8366s_vlan_set_vlan(struct rtl8366_smi *smi, int enable)
 {
        return rtl8366_smi_rmwr(smi, RTL8366_CHIP_GLOBAL_CTRL_REG,


@@ -766,12 +840,11 @@ static int rtl8366s_vlan_set_4ktable(struct rtl8366_smi *smi, int enable)
 
 static int rtl8366s_reset_vlan(struct rtl8366_smi *smi)
 {
 
 static int rtl8366s_reset_vlan(struct rtl8366_smi *smi)
 {

-       struct rtl8366_vlan_4k vlan4k;

        struct rtl8366_vlan_mc vlanmc;
        int err;
        int i;
 
        struct rtl8366_vlan_mc vlanmc;
        int err;
        int i;
 

-       /* clear 16 VLAN member configuration */
+       /* clear VLAN member configurations */

        vlanmc.vid = 0;
        vlanmc.priority = 0;
        vlanmc.member = 0;
        vlanmc.vid = 0;
        vlanmc.priority = 0;
        vlanmc.member = 0;


@@ -783,18 +856,18 @@ static int rtl8366s_reset_vlan(struct rtl8366_smi *smi)
                        return err;
        }
 
                        return err;
        }
 

-       /* Set a default VLAN with vid 1 to 4K table for all ports */
-       vlan4k.vid = 1;
-       vlan4k.member = RTL8366_PORT_ALL;
-       vlan4k.untag = RTL8366_PORT_ALL;
-       vlan4k.fid = 0;
-       err = rtl8366s_set_vlan_4k(smi, &vlan4k);
-       if (err)
-               return err;
-
-       /* Set all ports PVID to default VLAN */

        for (i = 0; i < RTL8366_NUM_PORTS; i++) {
        for (i = 0; i < RTL8366_NUM_PORTS; i++) {

-               err = rtl8366s_set_vlan_port_pvid(smi, i, 0);
+               if (i == RTL8366_PORT_CPU)
+                       continue;
+
+               err = rtl8366s_set_vlan(smi, (i + 1),
+                                        (1 << i) | RTL8366_PORT_CPU,
+                                        (1 << i) | RTL8366_PORT_CPU,
+                                        0);
+               if (err)
+                       return err;
+
+               err = rtl8366s_set_pvid(smi, i, (i + 1));

                if (err)
                        return err;
        }
                if (err)
                        return err;
        }


@@ -872,7 +945,7 @@ static ssize_t rtl8366s_read_debugfs_vlan(struct file *file,
 
                for (j = 0; j < RTL8366_NUM_PORTS; ++j) {
                        int index = 0;
 
                for (j = 0; j < RTL8366_NUM_PORTS; ++j) {
                        int index = 0;

-                       if (!rtl8366s_get_port_vlan_index(smi, j, &index)) {
+                       if (!rtl8366s_get_mc_index(smi, j, &index)) {

                                if (index == i)
                                        len += snprintf(buf + len,
                                                        sizeof(rtl->buf) - len,
                                if (index == i)
                                        len += snprintf(buf + len,
                                                        sizeof(rtl->buf) - len,


@@ -1167,43 +1240,35 @@ static int rtl8366s_sw_get_vlan_info(struct switch_dev *dev,
 {
        int i;
        u32 len = 0;
 {
        int i;
        u32 len = 0;

-       struct rtl8366_vlan_mc vlanmc;

        struct rtl8366_vlan_4k vlan4k;
        struct rtl8366s *rtl = sw_to_rtl8366s(dev);
        struct rtl8366_smi *smi = &rtl->smi;
        char *buf = rtl->buf;
        struct rtl8366_vlan_4k vlan4k;
        struct rtl8366s *rtl = sw_to_rtl8366s(dev);
        struct rtl8366_smi *smi = &rtl->smi;
        char *buf = rtl->buf;

+       int err;

 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 
        memset(buf, '\0', sizeof(rtl->buf));
 
 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 
        memset(buf, '\0', sizeof(rtl->buf));
 

-       rtl8366s_get_vlan_mc(smi, val->port_vlan, &vlanmc);
-       rtl8366s_get_vlan_4k(smi, vlanmc.vid, &vlan4k);
+       err = rtl8366s_get_vlan_4k(smi, val->port_vlan, &vlan4k);
+       if (err)
+               return err;

 
 

-       len += snprintf(buf + len, sizeof(rtl->buf) - len, "VLAN %d: Ports: ",
-                       val->port_vlan);
+       len += snprintf(buf + len, sizeof(rtl->buf) - len,
+                       "VLAN %d: Ports: '", vlan4k.vid);

 
 

-       for (i = 0; i < RTL8366_NUM_PORTS; ++i) {
-               int index = 0;
-               if (!rtl8366s_get_port_vlan_index(smi, i, &index) &&
-                   index == val->port_vlan)
-                       len += snprintf(buf + len, sizeof(rtl->buf) - len,
-                                       "%d", i);
+       for (i = 0; i < RTL8366_NUM_PORTS; i++) {
+               if (!(vlan4k.member & (1 << i)))
+                       continue;
+
+               len += snprintf(buf + len, sizeof(rtl->buf) - len, "%d%s", i,
+                               (vlan4k.untag & (1 << i)) ? "" : "t");

        }
        }

-       len += snprintf(buf + len, sizeof(rtl->buf) - len, "\n");

 
        len += snprintf(buf + len, sizeof(rtl->buf) - len,
 
        len += snprintf(buf + len, sizeof(rtl->buf) - len,

-                       "\t\t vid \t prio \t member \t untag \t fid\n");
-       len += snprintf(buf + len, sizeof(rtl->buf) - len, "\tMC:\t");
-       len += snprintf(buf + len, sizeof(rtl->buf) - len,
-                       "%d \t %d \t 0x%04x \t 0x%04x \t %d\n",
-                       vlanmc.vid, vlanmc.priority, vlanmc.member,
-                       vlanmc.untag, vlanmc.fid);
-       len += snprintf(buf + len, sizeof(rtl->buf) - len, "\t4K:\t");
-       len += snprintf(buf + len, sizeof(rtl->buf) - len,
-                       "%d \t  \t 0x%04x \t 0x%04x \t %d",
-                       vlan4k.vid, vlan4k.member, vlan4k.untag, vlan4k.fid);
+                       "', members=%04x, untag=%04x, fid=%u",
+                       vlan4k.member, vlan4k.untag, vlan4k.fid);

 
        val->value.s = buf;
        val->len = len;
 
        val->value.s = buf;
        val->len = len;


@@ -1304,23 +1369,23 @@ static int rtl8366s_sw_get_vlan_ports(struct switch_dev *dev,
                                      struct switch_val *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);
                                      struct switch_val *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);

-       struct rtl8366_vlan_mc vlanmc;

        struct switch_port *port;
        struct switch_port *port;

+       struct rtl8366_vlan_4k vlan4k;

        int i;
 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 
        int i;
 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 

-       rtl8366s_get_vlan_mc(smi, val->port_vlan, &vlanmc);
+       rtl8366s_get_vlan_4k(smi, val->port_vlan, &vlan4k);

 
        port = &val->value.ports[0];
        val->len = 0;
        for (i = 0; i < RTL8366_NUM_PORTS; i++) {
 
        port = &val->value.ports[0];
        val->len = 0;
        for (i = 0; i < RTL8366_NUM_PORTS; i++) {

-               if (!(vlanmc.member & BIT(i)))
+               if (!(vlan4k.member & BIT(i)))

                        continue;
 
                port->id = i;
                        continue;
 
                port->id = i;

-               port->flags = (vlanmc.untag & BIT(i)) ?
+               port->flags = (vlan4k.untag & BIT(i)) ?

                                        0 : BIT(SWITCH_PORT_FLAG_TAGGED);
                val->len++;
                port++;
                                        0 : BIT(SWITCH_PORT_FLAG_TAGGED);
                val->len++;
                port++;


@@ -1332,46 +1397,35 @@ static int rtl8366s_sw_set_vlan_ports(struct switch_dev *dev,
                                      struct switch_val *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);
                                      struct switch_val *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);

-       struct rtl8366_vlan_mc vlanmc;
-       struct rtl8366_vlan_4k vlan4k;

        struct switch_port *port;
        struct switch_port *port;

+       u32 member = 0;
+       u32 untag = 0;

        int i;
 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 
        int i;
 
        if (val->port_vlan == 0 || val->port_vlan >= RTL8366_NUM_VLANS)
                return -EINVAL;
 

-       rtl8366s_get_vlan_mc(smi, val->port_vlan, &vlanmc);
-       rtl8366s_get_vlan_4k(smi, vlanmc.vid, &vlan4k);
-
-       vlanmc.untag = 0;
-       vlanmc.member = 0;
-

        port = &val->value.ports[0];
        for (i = 0; i < val->len; i++, port++) {
        port = &val->value.ports[0];
        for (i = 0; i < val->len; i++, port++) {

-               vlanmc.member |= BIT(port->id);
+               member |= BIT(port->id);

 
                if (!(port->flags & BIT(SWITCH_PORT_FLAG_TAGGED)))
 
                if (!(port->flags & BIT(SWITCH_PORT_FLAG_TAGGED)))

-                       vlanmc.untag |= BIT(port->id);
+                       untag |= BIT(port->id);

        }
 
        }
 

-       vlan4k.member = vlanmc.member;
-       vlan4k.untag = vlanmc.untag;
-
-       rtl8366s_set_vlan_mc(smi, val->port_vlan, &vlanmc);
-       rtl8366s_set_vlan_4k(smi, &vlan4k);
-       return 0;
+       return rtl8366s_set_vlan(smi, val->port_vlan, member, untag, 0);

 }
 
 static int rtl8366s_sw_get_port_pvid(struct switch_dev *dev, int port, int *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);
 }
 
 static int rtl8366s_sw_get_port_pvid(struct switch_dev *dev, int port, int *val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);

-       return rtl8366s_get_vlan_port_pvid(smi, port, val);
+       return rtl8366s_get_pvid(smi, port, val);

 }
 
 static int rtl8366s_sw_set_port_pvid(struct switch_dev *dev, int port, int val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);
 }
 
 static int rtl8366s_sw_set_port_pvid(struct switch_dev *dev, int port, int val)
 {
        struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev);

-       return rtl8366s_set_vlan_port_pvid(smi, port, val);
+       return rtl8366s_set_pvid(smi, port, val);

 }
 
 static int rtl8366s_sw_reset_switch(struct switch_dev *dev)
 }
 
 static int rtl8366s_sw_reset_switch(struct switch_dev *dev)