1 files changed, 174 insertions, 120 deletions

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 3bb0b549d..4e758e97c 100644
--- 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;
 }
 
-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;
@@ -648,17 +647,68 @@ static int rtl8366s_get_port_vlan_index(struct rtl8366_smi *smi, int port,
 	       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;
 
-	err = rtl8366s_get_port_vlan_index(smi, port, &index);
+	err = rtl8366s_get_mc_index(smi, port, &index);
 	if (err)
 		return err;
 
@@ -670,87 +720,111 @@ static int rtl8366s_get_vlan_port_pvid(struct rtl8366_smi *smi, int port,
 	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;
+	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,
-		"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,
@@ -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)
 {
-	struct rtl8366_vlan_4k vlan4k;
 	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;
@@ -783,18 +856,18 @@ static int rtl8366s_reset_vlan(struct rtl8366_smi *smi)
 			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++) {
-		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;
 	}
@@ -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;
-			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,
@@ -1167,43 +1240,35 @@ static int rtl8366s_sw_get_vlan_info(struct switch_dev *dev,
 {
 	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;
+	int err;
 
 	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,
-			"\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;
@@ -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 rtl8366_vlan_mc vlanmc;
 	struct switch_port *port;
+	struct rtl8366_vlan_4k vlan4k;
 	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++) {
-		if (!(vlanmc.member & BIT(i)))
+		if (!(vlan4k.member & BIT(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++;
@@ -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 rtl8366_vlan_mc vlanmc;
-	struct rtl8366_vlan_4k vlan4k;
 	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;
 
-	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++) {
-		vlanmc.member |= BIT(port->id);
+		member |= BIT(port->id);
 
 		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);
-	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);
-	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)